<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.14"/>
<meta name="viewport" content="width=device-width, initial-scale=1"/>
<title>SPHinXsys: SPHINXsys/src/for_3D_build/particle_dynamics/solid_dynamics/polar_decomposition_3x3_impl.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/searchdata.js"></script>
<script type="text/javascript" src="search/search.js"></script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td id="projectlogo"><img alt="Logo" src="logo.png"/></td>
  <td id="projectalign" style="padding-left: 0.5em;">
   <div id="projectname">SPHinXsys
   &#160;<span id="projectnumber">alpha version</span>
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.14 -->
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
var searchBox = new SearchBox("searchBox", "search",false,'Search');
/* @license-end */
</script>
<script type="text/javascript" src="menudata.js"></script>
<script type="text/javascript" src="menu.js"></script>
<script type="text/javascript">
/* @license magnet:?xt=urn:btih:cf05388f2679ee054f2beb29a391d25f4e673ac3&amp;dn=gpl-2.0.txt GPL-v2 */
$(function() {
  initMenu('',true,false,'search.php','Search');
  $(document).ready(function() { init_search(); });
});
/* @license-end */</script>
<div id="main-nav"></div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
</div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_3dc1dabd28ba78e072d14d3fcfc90454.html">SPHINXsys</a></li><li class="navelem"><a class="el" href="dir_48ea983704c43b6d88c1fa3f4d09e369.html">src</a></li><li class="navelem"><a class="el" href="dir_836a4ac91a086ae3266ddeefe87700e6.html">for_3D_build</a></li><li class="navelem"><a class="el" href="dir_688778c02e8d0e8cfc36012f401c1130.html">particle_dynamics</a></li><li class="navelem"><a class="el" href="dir_a9d40bdc55e5e44802064deacc7908a4.html">solid_dynamics</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">polar_decomposition_3x3_impl.h</div>  </div>
</div><!--header-->
<div class="contents">
<div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="comment">// MIT License</span></div><div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;<span class="comment">// </span></div><div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="comment">// Copyright (c) 2017 Martin Bisson</span></div><div class="line"><a name="l00004"></a><span class="lineno">    4</span>&#160;<span class="comment">// </span></div><div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;<span class="comment">// Permission is hereby granted, free of charge, to any person obtaining a copy</span></div><div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="comment">// of this software and associated documentation files (the &quot;Software&quot;), to deal</span></div><div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="comment">// in the Software without restriction, including without limitation the rights</span></div><div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="comment">// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell</span></div><div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="comment">// copies of the Software, and to permit persons to whom the Software is</span></div><div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="comment">// furnished to do so, subject to the following conditions:</span></div><div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="comment">// </span></div><div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="comment">// The above copyright notice and this permission notice shall be included in all</span></div><div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="comment">// copies or substantial portions of the Software.</span></div><div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;<span class="comment">// </span></div><div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="comment">// THE SOFTWARE IS PROVIDED &quot;AS IS&quot;, WITHOUT WARRANTY OF ANY KIND, EXPRESS OR</span></div><div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="comment">// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,</span></div><div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;<span class="comment">// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE</span></div><div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="comment">// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER</span></div><div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;<span class="comment">// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,</span></div><div class="line"><a name="l00020"></a><span class="lineno">   20</span>&#160;<span class="comment">// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE</span></div><div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;<span class="comment">// SOFTWARE.</span></div><div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;<span class="preprocessor">#ifndef __POLAR_DECOMPOSITION_3X3_IMPL_H__</span></div><div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;<span class="preprocessor">#define __POLAR_DECOMPOSITION_3X3_IMPL_H__</span></div><div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;</div><div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;</div><div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;</div><div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;</div><div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;<span class="preprocessor">#include &quot;polar_decomposition_3x3_matrix.h&quot;</span></div><div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;</div><div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;<span class="preprocessor">#include &lt;algorithm&gt;</span></div><div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;</div><div class="line"><a name="l00032"></a><span class="lineno">   32</span>&#160;</div><div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;<span class="keyword">namespace </span><a class="code" href="namespacepolar.html">polar</a></div><div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;{</div><div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;</div><div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;</div><div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;    <span class="comment">// This part implements portions of the algorithms that are tailored</span></div><div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;    <span class="comment">// to just the specifics of what is needed.</span></div><div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;    <span class="keyword">namespace </span>detail</div><div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;    {</div><div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;</div><div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;</div><div class="line"><a name="l00043"></a><span class="lineno">   43</span>&#160;        <span class="comment">// This method is based on the Matlab implementation.</span></div><div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;        <span class="comment">// It computes the determinant of B matrix from an LU factorization with partial pivoting,</span></div><div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;        <span class="comment">// except that it actually computes it from the values of A, (B matrix is a function of A).</span></div><div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;        <span class="keyword">inline</span> TReal compute_b_determinant_from_a_matrix_lu_partial(<span class="keyword">const</span> matrix&lt;TReal, 3, 3&gt;&amp; A)</div><div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;        {</div><div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;            TReal b = 0;</div><div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;            {</div><div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;                TReal temp;</div><div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;                temp = (A(1,1)*A(2,2)-A(2,1)*A(1,2)); b += (temp * temp);</div><div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;                temp = (A(0,1)*A(2,2)-A(2,1)*A(0,2)); b += (temp * temp);</div><div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;                temp = (A(0,1)*A(1,2)-A(1,1)*A(0,2)); b += (temp * temp);</div><div class="line"><a name="l00055"></a><span class="lineno">   55</span>&#160;                temp = (A(0,0)*A(1,2)-A(1,0)*A(0,2)); b += (temp * temp);</div><div class="line"><a name="l00056"></a><span class="lineno">   56</span>&#160;                temp = (A(0,0)*A(2,2)-A(2,0)*A(0,2)); b += (temp * temp);</div><div class="line"><a name="l00057"></a><span class="lineno">   57</span>&#160;                temp = (A(1,0)*A(2,2)-A(2,0)*A(1,2)); b += (temp * temp);</div><div class="line"><a name="l00058"></a><span class="lineno">   58</span>&#160;                temp = (A(1,0)*A(2,1)-A(2,0)*A(1,1)); b += (temp * temp);</div><div class="line"><a name="l00059"></a><span class="lineno">   59</span>&#160;                temp = (A(0,0)*A(2,1)-A(2,0)*A(0,1)); b += (temp * temp);</div><div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;                temp = (A(0,0)*A(1,1)-A(1,0)*A(0,1)); b += (temp * temp);</div><div class="line"><a name="l00061"></a><span class="lineno">   61</span>&#160;                b = -4 * b + 1;</div><div class="line"><a name="l00062"></a><span class="lineno">   62</span>&#160;            }</div><div class="line"><a name="l00063"></a><span class="lineno">   63</span>&#160;            <span class="keywordflow">return</span> b;</div><div class="line"><a name="l00064"></a><span class="lineno">   64</span>&#160;        }</div><div class="line"><a name="l00065"></a><span class="lineno">   65</span>&#160;</div><div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;</div><div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;        <span class="comment">// This method is based on the Matlab implementation.</span></div><div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;        <span class="comment">// It computes the determinant of A matrix from an LU factorization with partial pivoting.</span></div><div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;        <span class="keyword">inline</span> TReal compute_determinant_lu_partial(<span class="keyword">const</span> matrix&lt;TReal, 3, 3&gt;&amp; A, TReal&amp; d)</div><div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;        {</div><div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;            TReal dd; <span class="comment">// Determinant, d is the sign of the determinant.</span></div><div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;            <span class="comment">// ANSME: Do we really need to keep track of the sign, or can&#39;t we just check it at the end?</span></div><div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;</div><div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;            matrix&lt;TReal, 3, 3&gt; AA;</div><div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;            <span class="keyword">const</span> TReal absA00 = math_utils&lt;TReal&gt;::fabs(A(0,0));</div><div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;            <span class="keyword">const</span> TReal absA01 = math_utils&lt;TReal&gt;::fabs(A(0,1));</div><div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;            <span class="keyword">const</span> TReal absA02 = math_utils&lt;TReal&gt;::fabs(A(0,2));</div><div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;            <span class="keywordflow">if</span> (absA01 &gt; absA02)</div><div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;            {</div><div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;                <span class="keywordflow">if</span> (absA00 &gt; absA01)</div><div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;                {</div><div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;                    AA = A;</div><div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;                    dd = 1;</div><div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;                }</div><div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;                <span class="keywordflow">else</span></div><div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;                {</div><div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;                    AA(0,0) = A(0,1);</div><div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;                    AA(1,0) = A(1,1);</div><div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;                    AA(2,0) = A(2,1);</div><div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;                    AA(0,1) = A(0,0);</div><div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;                    AA(1,1) = A(1,0);</div><div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;                    AA(2,1) = A(2,0);</div><div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;                    AA(0,2) = A(0,2);</div><div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;                    AA(1,2) = A(1,2);</div><div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;                    AA(2,2) = A(2,2);</div><div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;                    dd = -1;</div><div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;                }</div><div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;            }</div><div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;            <span class="keywordflow">else</span></div><div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;            {</div><div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;                <span class="keywordflow">if</span> (absA00 &gt; absA02)</div><div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;                {</div><div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;                    AA = A;</div><div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;                    dd = 1;</div><div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;                }</div><div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;                <span class="keywordflow">else</span></div><div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;                {</div><div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;                    AA(0,0) = A(0,2);</div><div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;                    AA(1,0) = A(1,2);</div><div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;                    AA(2,0) = A(2,2);</div><div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;                    AA(0,1) = A(0,1);</div><div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;                    AA(1,1) = A(1,1);</div><div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;                    AA(2,1) = A(2,1);</div><div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;                    AA(0,2) = A(0,0);</div><div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;                    AA(1,2) = A(1,0);</div><div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;                    AA(2,2) = A(2,0);</div><div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;                    dd = -1;</div><div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;                }</div><div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;            }</div><div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;</div><div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;            d = dd;</div><div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;            vector&lt;TReal, 3&gt; U;</div><div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;            U(0) = AA(0, 0);</div><div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;            <span class="keywordflow">if</span> (U(0) &lt; 0)</div><div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;                d = -d;</div><div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;</div><div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;            <span class="keyword">const</span> TReal m1 = AA(1,0) / AA(0,0);</div><div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;            <span class="keyword">const</span> TReal m2 = AA(2,0) / AA(0,0);</div><div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;            <span class="keyword">const</span> TReal AA00 = AA(1,1) - AA(0,1) * m1;</div><div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;            <span class="keyword">const</span> TReal AA10 = AA(2,1) - AA(0,1) * m2;</div><div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;            <span class="keyword">const</span> TReal AA01 = AA(1,2) - AA(0,2) * m1;</div><div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;            <span class="keyword">const</span> TReal AA11 = AA(2,2) - AA(0,2) * m2;</div><div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;</div><div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;            <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(AA00) &lt; math_utils&lt;TReal&gt;::fabs(AA01))</div><div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;            {</div><div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;                U(1) = AA01;</div><div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;                U(2) = AA10 - AA00 * AA11 / AA01;</div><div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;                dd = -dd;</div><div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;                d = -d;</div><div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;                <span class="keywordflow">if</span> (U(1) &lt; 0)</div><div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;                    d = -d;</div><div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;                <span class="keywordflow">if</span> (U(2) &lt; 0)</div><div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;                    d = -d;</div><div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;            }</div><div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;            <span class="keywordflow">else</span> <span class="keywordflow">if</span> (AA00 == 0)</div><div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;            {</div><div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;                U(1) = 0;</div><div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;                U(2) = 0;</div><div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;            }</div><div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;            <span class="keywordflow">else</span></div><div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;            {</div><div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;                U(1) = AA00;</div><div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;                U(2) = AA11 - AA01 * AA10 / AA00;</div><div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;                <span class="keywordflow">if</span> (U(1) &lt; 0)</div><div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;                    d = -d;</div><div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;                <span class="keywordflow">if</span> (U(2) &lt; 0)</div><div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;                    d = -d;</div><div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;            }</div><div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;</div><div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;            dd = dd * U(0) * U(1) * U(2);</div><div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;            <span class="keywordflow">if</span> (d == 0)</div><div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;                d = 1;</div><div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;</div><div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;            assert(((d &lt; 0) &amp;&amp; (dd &lt; 0)) || (dd &gt;= 0));</div><div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;            assert((d == 1) || (d == -1));</div><div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;</div><div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;            <span class="keywordflow">return</span> dd;</div><div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;        }</div><div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;</div><div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;</div><div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;        <span class="comment">// Swap rows.</span></div><div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> swap_rows(matrix&lt;TReal, 3, 3&gt;&amp; m, <span class="keyword">const</span> <span class="keywordtype">int</span> row0, <span class="keyword">const</span> <span class="keywordtype">int</span> row1)</div><div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;        {</div><div class="line"><a name="l00176"></a><span class="lineno">  176</span>&#160;            assert(row0 != row1);</div><div class="line"><a name="l00177"></a><span class="lineno">  177</span>&#160;            std::swap(m(0,row0), m(0,row1));</div><div class="line"><a name="l00178"></a><span class="lineno">  178</span>&#160;            std::swap(m(1,row0), m(1,row1));</div><div class="line"><a name="l00179"></a><span class="lineno">  179</span>&#160;            std::swap(m(2,row0), m(2,row1));</div><div class="line"><a name="l00180"></a><span class="lineno">  180</span>&#160;        }</div><div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> swap_rows(matrix&lt;TReal, 4, 4&gt;&amp; m, <span class="keyword">const</span> <span class="keywordtype">int</span> row0, <span class="keyword">const</span> <span class="keywordtype">int</span> row1)</div><div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;        {</div><div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;            assert(row0 != row1);</div><div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;            std::swap(m(0,row0), m(0,row1));</div><div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;            std::swap(m(1,row0), m(1,row1));</div><div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;            std::swap(m(2,row0), m(2,row1));</div><div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;            std::swap(m(3,row0), m(3,row1));</div><div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;        }</div><div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;</div><div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;        <span class="comment">// Swap columns.</span></div><div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> swap_columns(matrix&lt;TReal, 3, 3&gt;&amp; m, <span class="keyword">const</span> <span class="keywordtype">int</span> column0, <span class="keyword">const</span> <span class="keywordtype">int</span> column1)</div><div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;        {</div><div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;            assert(column0 != column1);</div><div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;            std::swap(m(column0,0), m(column1,0));</div><div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;            std::swap(m(column0,1), m(column1,1));</div><div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;            std::swap(m(column0,2), m(column1,2));</div><div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;        }</div><div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> swap_columns(matrix&lt;TReal, 4, 4&gt;&amp; m, <span class="keyword">const</span> <span class="keywordtype">int</span> column0, <span class="keyword">const</span> <span class="keywordtype">int</span> column1)</div><div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;        {</div><div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;            assert(column0 != column1);</div><div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;            std::swap(m(column0,0), m(column1,0));</div><div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;            std::swap(m(column0,1), m(column1,1));</div><div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;            std::swap(m(column0,2), m(column1,2));</div><div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;            std::swap(m(column0,3), m(column1,3));</div><div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;        }</div><div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;</div><div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;</div><div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;        <span class="comment">// This method is based on the Matlab implementation.</span></div><div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;        <span class="comment">// It computes an LDL^T factorization with diagonal pivoting, P^T Bs P = L D L^T.</span></div><div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;        <span class="comment">// This method modifies the Bs matrix.</span></div><div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;        <span class="comment">// Note: The method does not fill the whole L matrix, just the lower left part.</span></div><div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;        <span class="comment">// The caller should assume:</span></div><div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;        <span class="comment">//       - L(i,i) == 1</span></div><div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;        <span class="comment">//       - L(i,j) == 0 for i &gt; j</span></div><div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> compute_ldlt_factorization_diagonal(matrix&lt;TReal, 4, 4&gt;&amp; L, vector&lt;TReal, 4&gt;&amp; D, vector&lt;int, 4&gt;&amp; p, matrix&lt;TReal, 4, 4&gt;&amp; Bs)</div><div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;        {</div><div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;            p(0) = 0;</div><div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;            p(1) = 1;</div><div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;            p(2) = 2;</div><div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;            p(3) = 3;</div><div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;</div><div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;            <span class="comment">// ANSME: Should we compare absolute values to pick which row to pivot?</span></div><div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;            <span class="comment">//        This whole code could be refactored and improved to be more coherent</span></div><div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;            <span class="comment">//        from one step to another.</span></div><div class="line"><a name="l00229"></a><span class="lineno">  229</span>&#160;</div><div class="line"><a name="l00230"></a><span class="lineno">  230</span>&#160;            <span class="comment">// First step.</span></div><div class="line"><a name="l00231"></a><span class="lineno">  231</span>&#160;            {</div><div class="line"><a name="l00232"></a><span class="lineno">  232</span>&#160;                <span class="keywordtype">int</span> r = 3;</div><div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;                <span class="keywordflow">if</span> (Bs(3,3) &lt; Bs(2,2))</div><div class="line"><a name="l00234"></a><span class="lineno">  234</span>&#160;                    r = 2;</div><div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;                <span class="keywordflow">if</span> (Bs(r,r) &lt; Bs(1,1))</div><div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;                    r = 1;</div><div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;</div><div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;                <span class="keywordflow">if</span> (Bs(r,r) &gt; Bs(0,0))</div><div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;                {</div><div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;                    std::swap(p(0), p(r));</div><div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;                    swap_rows(Bs, 0, r);</div><div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;                    swap_columns(Bs, 0, r);</div><div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;                }</div><div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;</div><div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;                D(0) = Bs(0,0);</div><div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;                L(0,1) = Bs(0,1) / D(0);</div><div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;                L(0,2) = Bs(0,2) / D(0);</div><div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;                L(0,3) = Bs(0,3) / D(0);</div><div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;</div><div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;                Bs(1,1) = Bs(1,1) - L(0,1) * Bs(1,0);</div><div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;                Bs(1,2) = Bs(1,2) - L(0,1) * Bs(2,0);</div><div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;                Bs(2,1) = Bs(1,2);</div><div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;                Bs(1,3) = Bs(1,3) - L(0,1) * Bs(3,0);</div><div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;                Bs(3,1) = Bs(1,3);</div><div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;</div><div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;                Bs(2,2) = Bs(2,2) - L(0,2) * Bs(2,0);</div><div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;                Bs(2,3) = Bs(2,3) - L(0,2) * Bs(3,0);</div><div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;                Bs(3,2) = Bs(2,3);</div><div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;</div><div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;                Bs(3,3) = Bs(3,3) - L(0,3) * Bs(3,0);</div><div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;            }</div><div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;</div><div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;            <span class="comment">// Second step.</span></div><div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;            {</div><div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;                <span class="keywordtype">int</span> r = 3;</div><div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;                <span class="keywordflow">if</span> (Bs(3,3) &lt; Bs(2,2))</div><div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;                    r = 2;</div><div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;</div><div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;                <span class="keywordflow">if</span> (Bs(r,r) &gt; Bs(1,1))</div><div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;                {</div><div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;                    std::swap(p(1), p(r));</div><div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;                    swap_rows(Bs, 1, r);</div><div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;                    swap_columns(Bs, 1, r);</div><div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;<span class="preprocessor">                    #if 0</span></div><div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;                    swap_rows(L, 1, r);</div><div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;                    swap_columns(L, 1, r);</div><div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;<span class="preprocessor">                    #else</span></div><div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;                    <span class="comment">// Here, only the first column has been written, so we can swap just that.</span></div><div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;                    <span class="comment">//                 swap(1,2)      swap(1,3)</span></div><div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;                    <span class="comment">// | 1 0 0 0 |    | 1 0 0 0 |    | 1 0 0 0 |</span></div><div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;                    <span class="comment">// | a 1 0 0 |    | b 1 0 0 |    | c 1 0 0 |</span></div><div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;                    <span class="comment">// | b 0 1 0 |    | a 0 1 0 |    | b 0 1 0 |</span></div><div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;                    <span class="comment">// | c 0 0 1 |    | c 0 0 1 |    | a 0 0 1 |</span></div><div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;                    std::swap(L(0,1), L(0,r));</div><div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;<span class="preprocessor">                    #endif</span></div><div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;                }</div><div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;</div><div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;                D(1) = Bs(1,1);</div><div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;                L(1,2) = Bs(1,2) / D(1);</div><div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;                L(1,3) = Bs(1,3) / D(1);</div><div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;</div><div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;                Bs(2,2) = Bs(2,2) - L(1,2) * Bs(2,1);</div><div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;                Bs(2,3) = Bs(2,3) - L(1,2) * Bs(3,1);</div><div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;                Bs(3,2) = Bs(2,3);</div><div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;</div><div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;                Bs(3,3) = Bs(3,3) - L(1,3) * Bs(3,1);</div><div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;            }</div><div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;</div><div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;            <span class="comment">// Third step.</span></div><div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;            {</div><div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;                <span class="keywordflow">if</span> (Bs(2,2) &lt; Bs(3,3))</div><div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;                {</div><div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;                    D(2) = Bs(3,3);</div><div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;                    std::swap(p(2), p(3));</div><div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;                    swap_rows(Bs, 2, 3);</div><div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;                    swap_columns(Bs, 2, 3);</div><div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;<span class="preprocessor">                    #if 0</span></div><div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;                    swap_rows(L, 2, 3);</div><div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;                    swap_columns(L, 2, 3);</div><div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;<span class="preprocessor">                    #else</span></div><div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;                    <span class="comment">// Here, only the first two columns has been written, so we can swap just that.</span></div><div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;                    <span class="comment">//                 swap(2,3)</span></div><div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;                    <span class="comment">// | 1 0 0 0 |    | 1 0 0 0 |</span></div><div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;                    <span class="comment">// | a 1 0 0 |    | a 1 0 0 |</span></div><div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;                    <span class="comment">// | b e 1 0 |    | c f 1 0 |</span></div><div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;                    <span class="comment">// | c f 0 1 |    | b e 0 1 |</span></div><div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;                    std::swap(L(0,2), L(0,3));</div><div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;                    std::swap(L(1,2), L(1,3));</div><div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;<span class="preprocessor">                    #endif</span></div><div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;                }</div><div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;                <span class="keywordflow">else</span></div><div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;                {</div><div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;                    D(2) = Bs(2,2);</div><div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;                }</div><div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;</div><div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;                L(2,3) = Bs(2,3) / D(2);</div><div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;            }</div><div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;        }</div><div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;</div><div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;</div><div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;        <span class="comment">// This method is based on the Matlab implementation.</span></div><div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;        <span class="comment">// It computes the determinant of A matrix from an LU factorization with complete pivoting.</span></div><div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;        <span class="keyword">inline</span> TReal compute_determinant_lu_complete(<span class="keyword">const</span> matrix&lt;TReal, 3, 3&gt;&amp; A, TReal&amp; d, TReal&amp; u22)</div><div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;        {</div><div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;            TReal dd; <span class="comment">// Determinant, d is the sign of the determinant.</span></div><div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;            <span class="comment">// ANSME: Do we really need to keep track of the sign, or can&#39;t we just check it at the end?</span></div><div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;</div><div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;            matrix&lt;TReal, 3, 3&gt; AA = A;</div><div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;            {</div><div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;                <span class="keywordtype">int</span> r = 0;</div><div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;                <span class="keywordtype">int</span> c = 0;</div><div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;                dd = 1;</div><div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(0,1)) &gt; math_utils&lt;TReal&gt;::fabs(A(0,0)))</div><div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;                {</div><div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;                    r = 1;</div><div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;                }</div><div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(0,2)) &gt; math_utils&lt;TReal&gt;::fabs(A(c,r)))</div><div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;                {</div><div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;                    r = 2;</div><div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;                }</div><div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(1,0)) &gt; math_utils&lt;TReal&gt;::fabs(A(c,r)))</div><div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;                {</div><div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;                    r = 0;</div><div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;                    c = 1;</div><div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;                }</div><div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(1,1)) &gt; math_utils&lt;TReal&gt;::fabs(A(c,r)))</div><div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;                {</div><div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;                    r = 1;</div><div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;                    c = 1;</div><div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;                }</div><div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(1,2)) &gt; math_utils&lt;TReal&gt;::fabs(A(c,r)))</div><div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;                {</div><div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;                    r = 2;</div><div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;                    c = 1;</div><div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;                }</div><div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(2,0)) &gt; math_utils&lt;TReal&gt;::fabs(A(c,r)))</div><div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;                {</div><div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;                    r = 0;</div><div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;                    c = 2;</div><div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;                }</div><div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(2,1)) &gt; math_utils&lt;TReal&gt;::fabs(A(c,r)))</div><div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;                {</div><div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;                    r = 1;</div><div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;                    c = 2;</div><div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;                }</div><div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(A(2,2)) &gt; math_utils&lt;TReal&gt;::fabs(A(c,r)))</div><div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;                {</div><div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;                    r = 2;</div><div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160;                    c = 2;</div><div class="line"><a name="l00381"></a><span class="lineno">  381</span>&#160;                }</div><div class="line"><a name="l00382"></a><span class="lineno">  382</span>&#160;</div><div class="line"><a name="l00383"></a><span class="lineno">  383</span>&#160;                <span class="keywordflow">if</span> (r &gt; 0)</div><div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;                {</div><div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;                    swap_rows(AA, 0, r);</div><div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;                    dd = -1;</div><div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;                }</div><div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;                <span class="keywordflow">if</span> (c &gt; 0)</div><div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;                {</div><div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;                    swap_columns(AA, 0, c);</div><div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;                    dd = -dd;</div><div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;                }</div><div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;            }</div><div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;</div><div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;            vector&lt;TReal, 3&gt; U;</div><div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160;            U(0) = AA(0,0);</div><div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160;</div><div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;            matrix&lt; TReal, 2, 2&gt; AA_;</div><div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;            <span class="comment">// In case the whole matrix is 0, we add a small value.</span></div><div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;            <span class="keyword">const</span> TReal m1 = AA(1,0) / (AA(0,0) + (std::numeric_limits&lt;TReal&gt;::min)());</div><div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;            <span class="keyword">const</span> TReal m2 = AA(2,0) / (AA(0,0) + (std::numeric_limits&lt;TReal&gt;::min)());</div><div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;            AA_(0,0) = AA(1,1) - AA(0,1) * m1;</div><div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;            AA_(1,0) = AA(2,1) - AA(0,1) * m2;</div><div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;            AA_(0,1) = AA(1,2) - AA(0,2) * m1;</div><div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;            AA_(1,1) = AA(2,2) - AA(0,2) * m2;</div><div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;</div><div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;            {</div><div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;                <span class="keywordtype">int</span> r = 0;</div><div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160;                <span class="keywordtype">int</span> c = 0;</div><div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(AA_(0,1)) &gt; math_utils&lt;TReal&gt;::fabs(AA_(0,0)))</div><div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;                {</div><div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;                    r = 1;</div><div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;                }</div><div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(AA_(1,0)) &gt; math_utils&lt;TReal&gt;::fabs(AA_(c,r)))</div><div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160;                {</div><div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;                    r = 0;</div><div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;                    c = 1;</div><div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;                }</div><div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;                <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(AA_(1,1)) &gt; math_utils&lt;TReal&gt;::fabs(AA_(c,r)))</div><div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;                {</div><div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;                    r = 1;</div><div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;                    c = 1;</div><div class="line"><a name="l00423"></a><span class="lineno">  423</span>&#160;                }</div><div class="line"><a name="l00424"></a><span class="lineno">  424</span>&#160;</div><div class="line"><a name="l00425"></a><span class="lineno">  425</span>&#160;                <span class="keywordflow">if</span> (r &gt; 0)</div><div class="line"><a name="l00426"></a><span class="lineno">  426</span>&#160;                {</div><div class="line"><a name="l00427"></a><span class="lineno">  427</span>&#160;                    dd = -dd;</div><div class="line"><a name="l00428"></a><span class="lineno">  428</span>&#160;                }</div><div class="line"><a name="l00429"></a><span class="lineno">  429</span>&#160;                <span class="keywordflow">if</span> (c &gt; 0)</div><div class="line"><a name="l00430"></a><span class="lineno">  430</span>&#160;                {</div><div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;                    dd = -dd;</div><div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;                }</div><div class="line"><a name="l00433"></a><span class="lineno">  433</span>&#160;                U(1) = AA_(c,r);</div><div class="line"><a name="l00434"></a><span class="lineno">  434</span>&#160;                <span class="keywordflow">if</span> (U(1) == 0)</div><div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;                {</div><div class="line"><a name="l00436"></a><span class="lineno">  436</span>&#160;                    U(2) = 0;</div><div class="line"><a name="l00437"></a><span class="lineno">  437</span>&#160;                }</div><div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;                <span class="keywordflow">else</span></div><div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;                {</div><div class="line"><a name="l00440"></a><span class="lineno">  440</span>&#160;                    U(2) = AA_(1-c,1-r) - AA_(1-c,r) * AA_(c,1-r) / U(1);</div><div class="line"><a name="l00441"></a><span class="lineno">  441</span>&#160;                }</div><div class="line"><a name="l00442"></a><span class="lineno">  442</span>&#160;            }</div><div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;</div><div class="line"><a name="l00444"></a><span class="lineno">  444</span>&#160;            d = dd;</div><div class="line"><a name="l00445"></a><span class="lineno">  445</span>&#160;            dd = dd * U(0) * U(1) * U(2);</div><div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;            <span class="keywordflow">if</span> (U(0) &lt; 0)</div><div class="line"><a name="l00447"></a><span class="lineno">  447</span>&#160;                d = -d;</div><div class="line"><a name="l00448"></a><span class="lineno">  448</span>&#160;            <span class="keywordflow">if</span> (U(1) &lt; 0)</div><div class="line"><a name="l00449"></a><span class="lineno">  449</span>&#160;                d = -d;</div><div class="line"><a name="l00450"></a><span class="lineno">  450</span>&#160;            <span class="keywordflow">if</span> (U(2) &lt; 0)</div><div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;                d = -d;</div><div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;</div><div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;            u22 = U(1);</div><div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;</div><div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;            assert(((d &lt; 0) &amp;&amp; (dd &lt; 0)) || (dd &gt;= 0));</div><div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;            assert((d == 1) || (d == -1));</div><div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;</div><div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;            <span class="keywordflow">return</span> dd;</div><div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;        }</div><div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160;</div><div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;</div><div class="line"><a name="l00462"></a><span class="lineno">  462</span>&#160;        <span class="comment">// This method is based on the Matlab implementation.</span></div><div class="line"><a name="l00463"></a><span class="lineno">  463</span>&#160;        <span class="comment">// It computes an LDL^T factorization by block LDL^T factorization with Bunch-Parlett pivoting.</span></div><div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;        <span class="comment">// This method modifies the Bs matrix.</span></div><div class="line"><a name="l00465"></a><span class="lineno">  465</span>&#160;        <span class="comment">// Note: The method does not fill the whole L matrix, just the lower left part.</span></div><div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;        <span class="comment">// The caller should assume:</span></div><div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;        <span class="comment">//       - L(i,i) == 1</span></div><div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;        <span class="comment">//       - L(i,j) == 0 for i &gt; j</span></div><div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;        <span class="comment">//       - L(2,3) == 0</span></div><div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160;        <span class="comment">// FIXME: Not sure we actually need a 4x4 matrix for D.</span></div><div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> compute_ldlt_factorization_bunch_parlett(matrix&lt;TReal, 4, 4&gt;&amp; L, matrix&lt;TReal, 4, 4&gt;&amp; D, vector&lt;int, 4&gt;&amp; p, matrix&lt;TReal, 4, 4&gt;&amp; Bs)</div><div class="line"><a name="l00473"></a><span class="lineno">  473</span>&#160;        {</div><div class="line"><a name="l00474"></a><span class="lineno">  474</span>&#160;            p(0) = 0;</div><div class="line"><a name="l00475"></a><span class="lineno">  475</span>&#160;            p(1) = 1;</div><div class="line"><a name="l00476"></a><span class="lineno">  476</span>&#160;            p(2) = 2;</div><div class="line"><a name="l00477"></a><span class="lineno">  477</span>&#160;            p(3) = 3;</div><div class="line"><a name="l00478"></a><span class="lineno">  478</span>&#160;</div><div class="line"><a name="l00479"></a><span class="lineno">  479</span>&#160;            <span class="comment">// ANSME: Should we compare absolute values to pick which row to pivot?</span></div><div class="line"><a name="l00480"></a><span class="lineno">  480</span>&#160;            <span class="comment">//        This whole code could be refactored and improved to be more coherent</span></div><div class="line"><a name="l00481"></a><span class="lineno">  481</span>&#160;            <span class="comment">//        from one step to another.</span></div><div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;</div><div class="line"><a name="l00483"></a><span class="lineno">  483</span>&#160;            <span class="comment">// First step.</span></div><div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;            {</div><div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160;                <span class="keywordtype">int</span> r = 3;</div><div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;                <span class="keywordflow">if</span> (Bs(3,3) &lt; Bs(2,2))</div><div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160;                    r = 2;</div><div class="line"><a name="l00488"></a><span class="lineno">  488</span>&#160;                <span class="keywordflow">if</span> (Bs(r,r) &lt; Bs(1,1))</div><div class="line"><a name="l00489"></a><span class="lineno">  489</span>&#160;                    r = 1;</div><div class="line"><a name="l00490"></a><span class="lineno">  490</span>&#160;</div><div class="line"><a name="l00491"></a><span class="lineno">  491</span>&#160;                <span class="keywordflow">if</span> (Bs(r,r) &gt; Bs(0,0))</div><div class="line"><a name="l00492"></a><span class="lineno">  492</span>&#160;                {</div><div class="line"><a name="l00493"></a><span class="lineno">  493</span>&#160;                    std::swap(p(0), p(r));</div><div class="line"><a name="l00494"></a><span class="lineno">  494</span>&#160;                    swap_rows(Bs, 0, r);</div><div class="line"><a name="l00495"></a><span class="lineno">  495</span>&#160;                    swap_columns(Bs, 0, r);</div><div class="line"><a name="l00496"></a><span class="lineno">  496</span>&#160;                }</div><div class="line"><a name="l00497"></a><span class="lineno">  497</span>&#160;</div><div class="line"><a name="l00498"></a><span class="lineno">  498</span>&#160;                D(0,0) = Bs(0,0);</div><div class="line"><a name="l00499"></a><span class="lineno">  499</span>&#160;                L(0,1) = Bs(0,1) / D(0,0);</div><div class="line"><a name="l00500"></a><span class="lineno">  500</span>&#160;                L(0,2) = Bs(0,2) / D(0,0);</div><div class="line"><a name="l00501"></a><span class="lineno">  501</span>&#160;                L(0,3) = Bs(0,3) / D(0,0);</div><div class="line"><a name="l00502"></a><span class="lineno">  502</span>&#160;</div><div class="line"><a name="l00503"></a><span class="lineno">  503</span>&#160;                Bs(1,1) = Bs(1,1) - L(0,1) * Bs(1,0);</div><div class="line"><a name="l00504"></a><span class="lineno">  504</span>&#160;                Bs(1,2) = Bs(1,2) - L(0,1) * Bs(2,0);</div><div class="line"><a name="l00505"></a><span class="lineno">  505</span>&#160;                Bs(2,1) = Bs(1,2);</div><div class="line"><a name="l00506"></a><span class="lineno">  506</span>&#160;                Bs(1,3) = Bs(1,3) - L(0,1) * Bs(3,0);</div><div class="line"><a name="l00507"></a><span class="lineno">  507</span>&#160;                Bs(3,1) = Bs(1,3);</div><div class="line"><a name="l00508"></a><span class="lineno">  508</span>&#160;</div><div class="line"><a name="l00509"></a><span class="lineno">  509</span>&#160;                Bs(2,2) = Bs(2,2) - L(0,2) * Bs(2,0);</div><div class="line"><a name="l00510"></a><span class="lineno">  510</span>&#160;                Bs(2,3) = Bs(2,3) - L(0,2) * Bs(3,0);</div><div class="line"><a name="l00511"></a><span class="lineno">  511</span>&#160;                Bs(3,2) = Bs(2,3);</div><div class="line"><a name="l00512"></a><span class="lineno">  512</span>&#160;</div><div class="line"><a name="l00513"></a><span class="lineno">  513</span>&#160;                Bs(3,3) = Bs(3,3) - L(0,3) * Bs(3,0);</div><div class="line"><a name="l00514"></a><span class="lineno">  514</span>&#160;            }</div><div class="line"><a name="l00515"></a><span class="lineno">  515</span>&#160;</div><div class="line"><a name="l00516"></a><span class="lineno">  516</span>&#160;            <span class="comment">// Second step.</span></div><div class="line"><a name="l00517"></a><span class="lineno">  517</span>&#160;            {</div><div class="line"><a name="l00518"></a><span class="lineno">  518</span>&#160;                <span class="keywordtype">int</span> r = 2;</div><div class="line"><a name="l00519"></a><span class="lineno">  519</span>&#160;                <span class="keywordflow">if</span> (Bs(2,2) &lt; Bs(1,1))</div><div class="line"><a name="l00520"></a><span class="lineno">  520</span>&#160;                    r = 1;</div><div class="line"><a name="l00521"></a><span class="lineno">  521</span>&#160;</div><div class="line"><a name="l00522"></a><span class="lineno">  522</span>&#160;                <span class="keywordflow">if</span> (Bs(r,r) &gt; Bs(0,0))</div><div class="line"><a name="l00523"></a><span class="lineno">  523</span>&#160;                {</div><div class="line"><a name="l00524"></a><span class="lineno">  524</span>&#160;                    std::swap(p(1), p(r));</div><div class="line"><a name="l00525"></a><span class="lineno">  525</span>&#160;                    swap_rows(Bs, 1, r);</div><div class="line"><a name="l00526"></a><span class="lineno">  526</span>&#160;                    swap_columns(Bs, 1, r);</div><div class="line"><a name="l00527"></a><span class="lineno">  527</span>&#160;<span class="preprocessor">                    #if 0</span></div><div class="line"><a name="l00528"></a><span class="lineno">  528</span>&#160;                    swap_rows(L, 1, r);</div><div class="line"><a name="l00529"></a><span class="lineno">  529</span>&#160;                    swap_columns(L, 1, r);</div><div class="line"><a name="l00530"></a><span class="lineno">  530</span>&#160;<span class="preprocessor">                    #else</span></div><div class="line"><a name="l00531"></a><span class="lineno">  531</span>&#160;                    <span class="comment">// Here, only the first column has been written, so we can swap just that.</span></div><div class="line"><a name="l00532"></a><span class="lineno">  532</span>&#160;                    <span class="comment">//                 swap(1,2)      swap(1,3)</span></div><div class="line"><a name="l00533"></a><span class="lineno">  533</span>&#160;                    <span class="comment">// | 1 0 0 0 |    | 1 0 0 0 |    | 1 0 0 0 |</span></div><div class="line"><a name="l00534"></a><span class="lineno">  534</span>&#160;                    <span class="comment">// | a 1 0 0 |    | b 1 0 0 |    | c 1 0 0 |</span></div><div class="line"><a name="l00535"></a><span class="lineno">  535</span>&#160;                    <span class="comment">// | b 0 1 0 |    | a 0 1 0 |    | b 0 1 0 |</span></div><div class="line"><a name="l00536"></a><span class="lineno">  536</span>&#160;                    <span class="comment">// | c 0 0 1 |    | c 0 0 1 |    | a 0 0 1 |</span></div><div class="line"><a name="l00537"></a><span class="lineno">  537</span>&#160;                    std::swap(L(0,1), L(0,r));</div><div class="line"><a name="l00538"></a><span class="lineno">  538</span>&#160;<span class="preprocessor">                    #endif</span></div><div class="line"><a name="l00539"></a><span class="lineno">  539</span>&#160;                }</div><div class="line"><a name="l00540"></a><span class="lineno">  540</span>&#160;</div><div class="line"><a name="l00541"></a><span class="lineno">  541</span>&#160;                D(1,1) = Bs(1,1);</div><div class="line"><a name="l00542"></a><span class="lineno">  542</span>&#160;                L(1,2) = Bs(1,2) / D(1,1);</div><div class="line"><a name="l00543"></a><span class="lineno">  543</span>&#160;                L(1,3) = Bs(1,3) / D(1,1);</div><div class="line"><a name="l00544"></a><span class="lineno">  544</span>&#160;</div><div class="line"><a name="l00545"></a><span class="lineno">  545</span>&#160;                D(2,2) = Bs(2,2) - L(1,2) * Bs(2,1);</div><div class="line"><a name="l00546"></a><span class="lineno">  546</span>&#160;                D(2,3) = Bs(2,3) - L(1,2) * Bs(3,1);</div><div class="line"><a name="l00547"></a><span class="lineno">  547</span>&#160;                D(3,2) = D(2,3);</div><div class="line"><a name="l00548"></a><span class="lineno">  548</span>&#160;</div><div class="line"><a name="l00549"></a><span class="lineno">  549</span>&#160;                D(3,3) = Bs(3,3) - L(1,3) * Bs(3,1);</div><div class="line"><a name="l00550"></a><span class="lineno">  550</span>&#160;            }</div><div class="line"><a name="l00551"></a><span class="lineno">  551</span>&#160;        }</div><div class="line"><a name="l00552"></a><span class="lineno">  552</span>&#160;</div><div class="line"><a name="l00553"></a><span class="lineno">  553</span>&#160;</div><div class="line"><a name="l00554"></a><span class="lineno">  554</span>&#160;        <span class="comment">// This computes the null space of the matrix, knowing that it is symmetric:</span></div><div class="line"><a name="l00555"></a><span class="lineno">  555</span>&#160;        <span class="comment">//</span></div><div class="line"><a name="l00556"></a><span class="lineno">  556</span>&#160;        <span class="comment">// | a    b |</span></div><div class="line"><a name="l00557"></a><span class="lineno">  557</span>&#160;        <span class="comment">// | b    c |</span></div><div class="line"><a name="l00558"></a><span class="lineno">  558</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00559"></a><span class="lineno">  559</span>&#160;        <span class="keyword">inline</span> vector&lt;TReal, 2&gt; compute_null_space(<span class="keyword">const</span> TReal a, <span class="keyword">const</span> TReal b, <span class="keyword">const</span> TReal c)</div><div class="line"><a name="l00560"></a><span class="lineno">  560</span>&#160;        {</div><div class="line"><a name="l00561"></a><span class="lineno">  561</span>&#160;            <span class="comment">// We assume that the matrix determinant is 0.</span></div><div class="line"><a name="l00562"></a><span class="lineno">  562</span>&#160;            assert(0 == a * c - b * b);</div><div class="line"><a name="l00563"></a><span class="lineno">  563</span>&#160;</div><div class="line"><a name="l00564"></a><span class="lineno">  564</span>&#160;            vector&lt;TReal, 2&gt; nullSpace;</div><div class="line"><a name="l00565"></a><span class="lineno">  565</span>&#160;            <span class="keywordflow">if</span> (a != 0)</div><div class="line"><a name="l00566"></a><span class="lineno">  566</span>&#160;            {</div><div class="line"><a name="l00567"></a><span class="lineno">  567</span>&#160;                <span class="comment">// Transform: R1 = R1 / a</span></div><div class="line"><a name="l00568"></a><span class="lineno">  568</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00569"></a><span class="lineno">  569</span>&#160;                <span class="comment">// | 1    b / a |</span></div><div class="line"><a name="l00570"></a><span class="lineno">  570</span>&#160;                <span class="comment">// | b    c     |</span></div><div class="line"><a name="l00571"></a><span class="lineno">  571</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00572"></a><span class="lineno">  572</span>&#160;                <span class="comment">// R2 = R2 - b R1</span></div><div class="line"><a name="l00573"></a><span class="lineno">  573</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00574"></a><span class="lineno">  574</span>&#160;                <span class="comment">// | 1    b / a         |</span></div><div class="line"><a name="l00575"></a><span class="lineno">  575</span>&#160;                <span class="comment">// | 0    c - b * b / a |</span></div><div class="line"><a name="l00576"></a><span class="lineno">  576</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00577"></a><span class="lineno">  577</span>&#160;                <span class="comment">// Because a != 0 and a * c - b * b == 0, we have</span></div><div class="line"><a name="l00578"></a><span class="lineno">  578</span>&#160;                <span class="comment">// 1 * x1 + b / a * x2 = 0</span></div><div class="line"><a name="l00579"></a><span class="lineno">  579</span>&#160;                <span class="comment">// a x1 + b x2 = 0</span></div><div class="line"><a name="l00580"></a><span class="lineno">  580</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00581"></a><span class="lineno">  581</span>&#160;                <span class="comment">// solution: [ b    -a ]^T</span></div><div class="line"><a name="l00582"></a><span class="lineno">  582</span>&#160;                nullSpace(0) = b;</div><div class="line"><a name="l00583"></a><span class="lineno">  583</span>&#160;                nullSpace(1) = -a;</div><div class="line"><a name="l00584"></a><span class="lineno">  584</span>&#160;            }</div><div class="line"><a name="l00585"></a><span class="lineno">  585</span>&#160;            <span class="keywordflow">else</span></div><div class="line"><a name="l00586"></a><span class="lineno">  586</span>&#160;            {</div><div class="line"><a name="l00587"></a><span class="lineno">  587</span>&#160;                <span class="comment">// Since a == 0 and the determinant is null we have:</span></div><div class="line"><a name="l00588"></a><span class="lineno">  588</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00589"></a><span class="lineno">  589</span>&#160;                <span class="comment">// 0 * c - b * b = 0</span></div><div class="line"><a name="l00590"></a><span class="lineno">  590</span>&#160;                <span class="comment">// b = 0</span></div><div class="line"><a name="l00591"></a><span class="lineno">  591</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00592"></a><span class="lineno">  592</span>&#160;                <span class="comment">// If b == 0</span></div><div class="line"><a name="l00593"></a><span class="lineno">  593</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00594"></a><span class="lineno">  594</span>&#160;                <span class="comment">// We have:</span></div><div class="line"><a name="l00595"></a><span class="lineno">  595</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00596"></a><span class="lineno">  596</span>&#160;                <span class="comment">// | 0    0 |</span></div><div class="line"><a name="l00597"></a><span class="lineno">  597</span>&#160;                <span class="comment">// | b    c |</span></div><div class="line"><a name="l00598"></a><span class="lineno">  598</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00599"></a><span class="lineno">  599</span>&#160;                <span class="comment">// b x1 + c x2 = 0</span></div><div class="line"><a name="l00600"></a><span class="lineno">  600</span>&#160;                <span class="comment">// solution: [ c    -b ]^T</span></div><div class="line"><a name="l00601"></a><span class="lineno">  601</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00602"></a><span class="lineno">  602</span>&#160;                <span class="comment">// (With b == 0, [ c    0 ]^T).</span></div><div class="line"><a name="l00603"></a><span class="lineno">  603</span>&#160;                <span class="comment">//</span></div><div class="line"><a name="l00604"></a><span class="lineno">  604</span>&#160;                assert(a == 0);</div><div class="line"><a name="l00605"></a><span class="lineno">  605</span>&#160;                assert(b == 0);</div><div class="line"><a name="l00606"></a><span class="lineno">  606</span>&#160;                assert(c != 0);</div><div class="line"><a name="l00607"></a><span class="lineno">  607</span>&#160;                nullSpace(0) = c;</div><div class="line"><a name="l00608"></a><span class="lineno">  608</span>&#160;                nullSpace(1) = -b;</div><div class="line"><a name="l00609"></a><span class="lineno">  609</span>&#160;            }</div><div class="line"><a name="l00610"></a><span class="lineno">  610</span>&#160;</div><div class="line"><a name="l00611"></a><span class="lineno">  611</span>&#160;            <span class="keywordflow">return</span> nullSpace;</div><div class="line"><a name="l00612"></a><span class="lineno">  612</span>&#160;        }</div><div class="line"><a name="l00613"></a><span class="lineno">  613</span>&#160;</div><div class="line"><a name="l00614"></a><span class="lineno">  614</span>&#160;</div><div class="line"><a name="l00615"></a><span class="lineno">  615</span>&#160;        <span class="comment">// These methods are used for optimized operations in reverse iteration with LDL^T.</span></div><div class="line"><a name="l00616"></a><span class="lineno">  616</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00617"></a><span class="lineno">  617</span>&#160;        <span class="keyword">inline</span> vector&lt;TReal, 4&gt; multiply_il_v(</div><div class="line"><a name="l00618"></a><span class="lineno">  618</span>&#160;            <span class="keyword">const</span> TReal IL01, <span class="keyword">const</span> TReal IL02, <span class="keyword">const</span> TReal IL03, <span class="keyword">const</span> TReal IL12, <span class="keyword">const</span> TReal IL13,</div><div class="line"><a name="l00619"></a><span class="lineno">  619</span>&#160;            <span class="keyword">const</span> vector&lt;TReal, 4&gt;&amp; v</div><div class="line"><a name="l00620"></a><span class="lineno">  620</span>&#160;            )</div><div class="line"><a name="l00621"></a><span class="lineno">  621</span>&#160;        {</div><div class="line"><a name="l00622"></a><span class="lineno">  622</span>&#160;            <span class="comment">// {{1,0,0,0},{m_12,1,0,0},{m_13,m_23,1,0},{m_14,m_24,0,1}} * {{v_1},{v_2},{v_3},{v_4}}</span></div><div class="line"><a name="l00623"></a><span class="lineno">  623</span>&#160;            <span class="comment">//</span></div><div class="line"><a name="l00624"></a><span class="lineno">  624</span>&#160;            <span class="comment">// | 1       0       0       0 |       | v1 |       | v1                         |</span></div><div class="line"><a name="l00625"></a><span class="lineno">  625</span>&#160;            <span class="comment">// | IL01    1       0       0 |   *   | v2 |   =   | v1 * IL01 + v              |</span></div><div class="line"><a name="l00626"></a><span class="lineno">  626</span>&#160;            <span class="comment">// | IL02    IL12    1       0 |       | v3 |       | v1 * IL02 + v2 * IL12 + v3 |</span></div><div class="line"><a name="l00627"></a><span class="lineno">  627</span>&#160;            <span class="comment">// | IL03    IL13    0       1 |       | v4 |       | v1 * IL03 + v2 * IL13 + v4 |</span></div><div class="line"><a name="l00628"></a><span class="lineno">  628</span>&#160;            vector&lt;TReal, 4&gt; result;</div><div class="line"><a name="l00629"></a><span class="lineno">  629</span>&#160;            result(0) = v(0);</div><div class="line"><a name="l00630"></a><span class="lineno">  630</span>&#160;            result(1) = v(0) * IL01 + v(1);</div><div class="line"><a name="l00631"></a><span class="lineno">  631</span>&#160;            result(2) = v(0) * IL02 + v(1) * IL12 + v(2);</div><div class="line"><a name="l00632"></a><span class="lineno">  632</span>&#160;            result(3) = v(0) * IL03 + v(1) * IL13 + v(3);</div><div class="line"><a name="l00633"></a><span class="lineno">  633</span>&#160;            <span class="keywordflow">return</span> result;</div><div class="line"><a name="l00634"></a><span class="lineno">  634</span>&#160;        }</div><div class="line"><a name="l00635"></a><span class="lineno">  635</span>&#160;</div><div class="line"><a name="l00636"></a><span class="lineno">  636</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00637"></a><span class="lineno">  637</span>&#160;        <span class="keyword">inline</span> vector&lt;TReal, 4&gt; multiply_id_v(</div><div class="line"><a name="l00638"></a><span class="lineno">  638</span>&#160;            <span class="keyword">const</span> TReal ID00, <span class="keyword">const</span> TReal ID11, <span class="keyword">const</span> matrix&lt;TReal, 2, 2&gt;&amp; ID,</div><div class="line"><a name="l00639"></a><span class="lineno">  639</span>&#160;            <span class="keyword">const</span> vector&lt;TReal, 4&gt;&amp; v</div><div class="line"><a name="l00640"></a><span class="lineno">  640</span>&#160;            )</div><div class="line"><a name="l00641"></a><span class="lineno">  641</span>&#160;        {</div><div class="line"><a name="l00642"></a><span class="lineno">  642</span>&#160;            <span class="comment">// {{a_11,0,0,0},{0,a_22,0,0},{0,0,b_11,b_21},{0,0,b_12,b_22}} * {{v_1},{v_2},{v_3},{v_4}}</span></div><div class="line"><a name="l00643"></a><span class="lineno">  643</span>&#160;            <span class="comment">//</span></div><div class="line"><a name="l00644"></a><span class="lineno">  644</span>&#160;            <span class="comment">// | ID00    0       0          0       |       | v1 |       | v1 * ID00                   |</span></div><div class="line"><a name="l00645"></a><span class="lineno">  645</span>&#160;            <span class="comment">// | 0000    ID11    0          0       |   *   | v2 |   =   | v2 * ID11                   |</span></div><div class="line"><a name="l00646"></a><span class="lineno">  646</span>&#160;            <span class="comment">// | 0       0       ID(0,0)    ID(1,0) |       | v3 |       | v3 * ID(0,0) + v4 * ID(1,0) |</span></div><div class="line"><a name="l00647"></a><span class="lineno">  647</span>&#160;            <span class="comment">// | 0       0       ID(0,1)    ID(1,1) |       | v4 |       | v3 * ID(0,1) + v4 * ID(1,1) |</span></div><div class="line"><a name="l00648"></a><span class="lineno">  648</span>&#160;            vector&lt;TReal, 4&gt; result;</div><div class="line"><a name="l00649"></a><span class="lineno">  649</span>&#160;            result(0) = v(0) * ID00;</div><div class="line"><a name="l00650"></a><span class="lineno">  650</span>&#160;            result(1) = v(1) * ID11;</div><div class="line"><a name="l00651"></a><span class="lineno">  651</span>&#160;            result(2) = v(2) * ID(0,0) + v(3) * ID(1,0);</div><div class="line"><a name="l00652"></a><span class="lineno">  652</span>&#160;            result(3) = v(2) * ID(0,1) + v(3) * ID(1,1);</div><div class="line"><a name="l00653"></a><span class="lineno">  653</span>&#160;            <span class="keywordflow">return</span> result;</div><div class="line"><a name="l00654"></a><span class="lineno">  654</span>&#160;        }</div><div class="line"><a name="l00655"></a><span class="lineno">  655</span>&#160;</div><div class="line"><a name="l00656"></a><span class="lineno">  656</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00657"></a><span class="lineno">  657</span>&#160;        <span class="keyword">inline</span> vector&lt;TReal, 4&gt; multiply_v_il(</div><div class="line"><a name="l00658"></a><span class="lineno">  658</span>&#160;            <span class="keyword">const</span> vector&lt;TReal, 4&gt;&amp; v,</div><div class="line"><a name="l00659"></a><span class="lineno">  659</span>&#160;            <span class="keyword">const</span> TReal IL01, <span class="keyword">const</span> TReal IL02, <span class="keyword">const</span> TReal IL03, <span class="keyword">const</span> TReal IL12, <span class="keyword">const</span> TReal IL13</div><div class="line"><a name="l00660"></a><span class="lineno">  660</span>&#160;            )</div><div class="line"><a name="l00661"></a><span class="lineno">  661</span>&#160;        {</div><div class="line"><a name="l00662"></a><span class="lineno">  662</span>&#160;            <span class="comment">// Transpose[{{1,0,0,0},{m_12,1,0,0},{m_13,m_23,1,0},{m_14,m_24,0,1}}] * {{v_1},{v_2},{v_3},{v_4}}</span></div><div class="line"><a name="l00663"></a><span class="lineno">  663</span>&#160;            <span class="comment">// {v_1,v_2,v_3,v_4} * {{1,0,0,0},{m_12,1,0,0},{m_13,m_23,1,0},{m_14,m_24,0,1}}</span></div><div class="line"><a name="l00664"></a><span class="lineno">  664</span>&#160;            <span class="comment">//</span></div><div class="line"><a name="l00665"></a><span class="lineno">  665</span>&#160;            <span class="comment">// | v1 |^T   *   | 1       0       0       0 |       | v1 + v2 * IL01 + v3 * IL02 + v4 * IL03 |^T</span></div><div class="line"><a name="l00666"></a><span class="lineno">  666</span>&#160;            <span class="comment">// | v2 |         | IL01    1       0       0 |   =   | v2 + v3 * IL12 + v4 * IL13             |</span></div><div class="line"><a name="l00667"></a><span class="lineno">  667</span>&#160;            <span class="comment">// | v3 |         | IL02    IL12    1       0 |       | v3                                     |</span></div><div class="line"><a name="l00668"></a><span class="lineno">  668</span>&#160;            <span class="comment">// | v4 |         | IL03    IL13    0       1 |       | v4                                     |</span></div><div class="line"><a name="l00669"></a><span class="lineno">  669</span>&#160;            vector&lt;TReal, 4&gt; result;</div><div class="line"><a name="l00670"></a><span class="lineno">  670</span>&#160;            result(0) = v(0) + v(1) * IL01 + v(2) * IL02 + v(3) * IL03;</div><div class="line"><a name="l00671"></a><span class="lineno">  671</span>&#160;            result(1) = v(1) + v(2) * IL12 + v(3) * IL13;</div><div class="line"><a name="l00672"></a><span class="lineno">  672</span>&#160;            result(2) = v(2);</div><div class="line"><a name="l00673"></a><span class="lineno">  673</span>&#160;            result(3) = v(3);</div><div class="line"><a name="l00674"></a><span class="lineno">  674</span>&#160;            <span class="keywordflow">return</span> result;</div><div class="line"><a name="l00675"></a><span class="lineno">  675</span>&#160;        }</div><div class="line"><a name="l00676"></a><span class="lineno">  676</span>&#160;</div><div class="line"><a name="l00677"></a><span class="lineno">  677</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00678"></a><span class="lineno">  678</span>&#160;        <span class="keyword">inline</span> vector&lt;TReal, 4&gt; multiply_minus_v_d(</div><div class="line"><a name="l00679"></a><span class="lineno">  679</span>&#160;            <span class="keyword">const</span> vector&lt;TReal, 4&gt;&amp; v,</div><div class="line"><a name="l00680"></a><span class="lineno">  680</span>&#160;            <span class="keyword">const</span> matrix&lt;TReal, 4, 4&gt;&amp; D</div><div class="line"><a name="l00681"></a><span class="lineno">  681</span>&#160;            )</div><div class="line"><a name="l00682"></a><span class="lineno">  682</span>&#160;        {</div><div class="line"><a name="l00683"></a><span class="lineno">  683</span>&#160;            <span class="comment">// -{v1,v2,v3,v4} * {{D_11,0,0,0},{0,D_22,0,0},{0,0,D_33,D_43},{0,0,D_34,D_44}}</span></div><div class="line"><a name="l00684"></a><span class="lineno">  684</span>&#160;            <span class="comment">//</span></div><div class="line"><a name="l00685"></a><span class="lineno">  685</span>&#160;            <span class="comment">// - | v1 |^T   *   | D(0,0)    0         0            0      |       | -v1 * D(0,0)               |^T</span></div><div class="line"><a name="l00686"></a><span class="lineno">  686</span>&#160;            <span class="comment">//   | v2 |         | 0         D(1,1)    0            0      |   =   | -v2 * D(1,1)               |</span></div><div class="line"><a name="l00687"></a><span class="lineno">  687</span>&#160;            <span class="comment">//   | v3 |         | 0         0         D(2,2)       D(3,2) |       | -v3 * D(2,2) - v4 * D(2,3) |</span></div><div class="line"><a name="l00688"></a><span class="lineno">  688</span>&#160;            <span class="comment">//   | v4 |         | 0         0         D(2,3)       D(3,3) |       | -v4 * D(3,2) - v4 * D(3,3) |</span></div><div class="line"><a name="l00689"></a><span class="lineno">  689</span>&#160;            vector&lt;TReal, 4&gt; result;</div><div class="line"><a name="l00690"></a><span class="lineno">  690</span>&#160;            result(0) = -v(0) * D(0,0);</div><div class="line"><a name="l00691"></a><span class="lineno">  691</span>&#160;            result(1) = -v(1) * D(1,1);</div><div class="line"><a name="l00692"></a><span class="lineno">  692</span>&#160;            result(2) = -v(2) * D(2,2) - v(3) * D(2,3);</div><div class="line"><a name="l00693"></a><span class="lineno">  693</span>&#160;            result(3) = -v(2) * D(3,2) - v(3) * D(3,3);</div><div class="line"><a name="l00694"></a><span class="lineno">  694</span>&#160;            <span class="keywordflow">return</span> result;</div><div class="line"><a name="l00695"></a><span class="lineno">  695</span>&#160;        }</div><div class="line"><a name="l00696"></a><span class="lineno">  696</span>&#160;</div><div class="line"><a name="l00697"></a><span class="lineno">  697</span>&#160;</div><div class="line"><a name="l00698"></a><span class="lineno">  698</span>&#160;        <span class="comment">// Orthonormalization of matrices of the type:</span></div><div class="line"><a name="l00699"></a><span class="lineno">  699</span>&#160;        <span class="comment">//</span></div><div class="line"><a name="l00700"></a><span class="lineno">  700</span>&#160;        <span class="comment">// | v00    v10 |</span></div><div class="line"><a name="l00701"></a><span class="lineno">  701</span>&#160;        <span class="comment">// | v01    v11 |</span></div><div class="line"><a name="l00702"></a><span class="lineno">  702</span>&#160;        <span class="comment">// | 1      0   |</span></div><div class="line"><a name="l00703"></a><span class="lineno">  703</span>&#160;        <span class="comment">// | 0      1   |</span></div><div class="line"><a name="l00704"></a><span class="lineno">  704</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00705"></a><span class="lineno">  705</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> orthonormalize_v_with_qr(</div><div class="line"><a name="l00706"></a><span class="lineno">  706</span>&#160;            vector&lt;TReal, 4&gt;&amp; v0,</div><div class="line"><a name="l00707"></a><span class="lineno">  707</span>&#160;            vector&lt;TReal, 4&gt;&amp; v1,</div><div class="line"><a name="l00708"></a><span class="lineno">  708</span>&#160;            <span class="keyword">const</span> TReal v00, <span class="keyword">const</span> TReal v10, <span class="keyword">const</span> TReal v01, <span class="keyword">const</span> TReal v11</div><div class="line"><a name="l00709"></a><span class="lineno">  709</span>&#160;            )</div><div class="line"><a name="l00710"></a><span class="lineno">  710</span>&#160;        {</div><div class="line"><a name="l00711"></a><span class="lineno">  711</span>&#160;            <span class="comment">// The factorization was obtained symbolically by WolframAlpha</span></div><div class="line"><a name="l00712"></a><span class="lineno">  712</span>&#160;            <span class="comment">// by running the following query:</span></div><div class="line"><a name="l00713"></a><span class="lineno">  713</span>&#160;            <span class="comment">//</span></div><div class="line"><a name="l00714"></a><span class="lineno">  714</span>&#160;            <span class="comment">// QRDecomposition[{{v_11,v_21},{v_12,v_22},{1,0},{0,1}}]</span></div><div class="line"><a name="l00715"></a><span class="lineno">  715</span>&#160;            v0(0) = v00;</div><div class="line"><a name="l00716"></a><span class="lineno">  716</span>&#160;            v0(1) = v01;</div><div class="line"><a name="l00717"></a><span class="lineno">  717</span>&#160;            v0(2) = 1;</div><div class="line"><a name="l00718"></a><span class="lineno">  718</span>&#160;            v0(3) = 0;</div><div class="line"><a name="l00719"></a><span class="lineno">  719</span>&#160;            normalize(v0);</div><div class="line"><a name="l00720"></a><span class="lineno">  720</span>&#160;</div><div class="line"><a name="l00721"></a><span class="lineno">  721</span>&#160;            <span class="comment">// OPTME: We could reuse some of the multiplications.</span></div><div class="line"><a name="l00722"></a><span class="lineno">  722</span>&#160;            v1(0) = v10 + v01 * v01 * v10 - v00 * v01 * v11;</div><div class="line"><a name="l00723"></a><span class="lineno">  723</span>&#160;            v1(1) = v11 - v00 * v01 * v10 + v00 * v00 * v11;</div><div class="line"><a name="l00724"></a><span class="lineno">  724</span>&#160;            v1(2) = -v00 * v10 - v01 * v11;</div><div class="line"><a name="l00725"></a><span class="lineno">  725</span>&#160;            v1(3) = v00 * v00 + v01 * v01 + 1;</div><div class="line"><a name="l00726"></a><span class="lineno">  726</span>&#160;            normalize(v1);</div><div class="line"><a name="l00727"></a><span class="lineno">  727</span>&#160;        }</div><div class="line"><a name="l00728"></a><span class="lineno">  728</span>&#160;</div><div class="line"><a name="l00729"></a><span class="lineno">  729</span>&#160;</div><div class="line"><a name="l00730"></a><span class="lineno">  730</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00731"></a><span class="lineno">  731</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> orthonormalize_v_with_qr(</div><div class="line"><a name="l00732"></a><span class="lineno">  732</span>&#160;            vector&lt;TReal, 4&gt;&amp; v0,</div><div class="line"><a name="l00733"></a><span class="lineno">  733</span>&#160;            vector&lt;TReal, 4&gt;&amp; v1</div><div class="line"><a name="l00734"></a><span class="lineno">  734</span>&#160;            )</div><div class="line"><a name="l00735"></a><span class="lineno">  735</span>&#160;        {</div><div class="line"><a name="l00736"></a><span class="lineno">  736</span>&#160;            <span class="comment">// The factorization was obtained symbolically by WolframAlpha</span></div><div class="line"><a name="l00737"></a><span class="lineno">  737</span>&#160;            <span class="comment">// by running the following query:</span></div><div class="line"><a name="l00738"></a><span class="lineno">  738</span>&#160;            <span class="comment">//</span></div><div class="line"><a name="l00739"></a><span class="lineno">  739</span>&#160;            <span class="comment">// QRDecomposition[{{a,e},{b,f},{c,g},{d,h}}]</span></div><div class="line"><a name="l00740"></a><span class="lineno">  740</span>&#160;</div><div class="line"><a name="l00741"></a><span class="lineno">  741</span>&#160;            normalize(v0);</div><div class="line"><a name="l00742"></a><span class="lineno">  742</span>&#160;</div><div class="line"><a name="l00743"></a><span class="lineno">  743</span>&#160;            <span class="comment">// To avoid numerical stability issues when multiplying too big values in the solution,</span></div><div class="line"><a name="l00744"></a><span class="lineno">  744</span>&#160;            <span class="comment">// we scale down the second vector.</span></div><div class="line"><a name="l00745"></a><span class="lineno">  745</span>&#160;            TReal factor = v1(0);</div><div class="line"><a name="l00746"></a><span class="lineno">  746</span>&#160;            <span class="keywordflow">if</span> (factor &lt; math_utils&lt;TReal&gt;::fabs(v1(1)))</div><div class="line"><a name="l00747"></a><span class="lineno">  747</span>&#160;                factor = math_utils&lt;TReal&gt;::fabs(v1(1));</div><div class="line"><a name="l00748"></a><span class="lineno">  748</span>&#160;            <span class="keywordflow">if</span> (factor &lt; math_utils&lt;TReal&gt;::fabs(v1(2)))</div><div class="line"><a name="l00749"></a><span class="lineno">  749</span>&#160;                factor = math_utils&lt;TReal&gt;::fabs(v1(2));</div><div class="line"><a name="l00750"></a><span class="lineno">  750</span>&#160;            <span class="keywordflow">if</span> (factor &lt; math_utils&lt;TReal&gt;::fabs(v1(3)))</div><div class="line"><a name="l00751"></a><span class="lineno">  751</span>&#160;                factor = math_utils&lt;TReal&gt;::fabs(v1(3));</div><div class="line"><a name="l00752"></a><span class="lineno">  752</span>&#160;            factor = 1 / (factor + (std::numeric_limits&lt;TReal&gt;::min)());</div><div class="line"><a name="l00753"></a><span class="lineno">  753</span>&#160;</div><div class="line"><a name="l00754"></a><span class="lineno">  754</span>&#160;            <span class="keyword">const</span> TReal a = v0(0);</div><div class="line"><a name="l00755"></a><span class="lineno">  755</span>&#160;            <span class="keyword">const</span> TReal b = v0(1);</div><div class="line"><a name="l00756"></a><span class="lineno">  756</span>&#160;            <span class="keyword">const</span> TReal c = v0(2);</div><div class="line"><a name="l00757"></a><span class="lineno">  757</span>&#160;            <span class="keyword">const</span> TReal d = v0(3);</div><div class="line"><a name="l00758"></a><span class="lineno">  758</span>&#160;            <span class="keyword">const</span> TReal e = v1(0) * factor;</div><div class="line"><a name="l00759"></a><span class="lineno">  759</span>&#160;            <span class="keyword">const</span> TReal f = v1(1) * factor;</div><div class="line"><a name="l00760"></a><span class="lineno">  760</span>&#160;            <span class="keyword">const</span> TReal g = v1(2) * factor;</div><div class="line"><a name="l00761"></a><span class="lineno">  761</span>&#160;            <span class="keyword">const</span> TReal h = v1(3) * factor;</div><div class="line"><a name="l00762"></a><span class="lineno">  762</span>&#160;</div><div class="line"><a name="l00763"></a><span class="lineno">  763</span>&#160;            <span class="comment">// OPTME: We could reuse some of the multiplications.</span></div><div class="line"><a name="l00764"></a><span class="lineno">  764</span>&#160;            <span class="comment">// ANSME: Is there a more numerically stable way to do this?</span></div><div class="line"><a name="l00765"></a><span class="lineno">  765</span>&#160;            v1(0) = b*b*e + c*c*e + d*d*e - a*b*f - a*c*g - a*d*h;</div><div class="line"><a name="l00766"></a><span class="lineno">  766</span>&#160;            v1(1) = -a*b*e + a*a*f + c*c*f + d*d*f - b*c*g - b*d*h;</div><div class="line"><a name="l00767"></a><span class="lineno">  767</span>&#160;            v1(2) = -a*c*e - b*c*f + a*a*g + b*b*g + d*d*g - c*d*h;</div><div class="line"><a name="l00768"></a><span class="lineno">  768</span>&#160;            v1(3) = -a*d*e - b*d*f - c*d*g + a*a*h + b*b*h + c*c*h;</div><div class="line"><a name="l00769"></a><span class="lineno">  769</span>&#160;            normalize(v1);</div><div class="line"><a name="l00770"></a><span class="lineno">  770</span>&#160;        }</div><div class="line"><a name="l00771"></a><span class="lineno">  771</span>&#160;</div><div class="line"><a name="l00772"></a><span class="lineno">  772</span>&#160;</div><div class="line"><a name="l00773"></a><span class="lineno">  773</span>&#160;    }; <span class="comment">// End of namespace detail.</span></div><div class="line"><a name="l00774"></a><span class="lineno">  774</span>&#160;</div><div class="line"><a name="l00775"></a><span class="lineno">  775</span>&#160;</div><div class="line"><a name="l00776"></a><span class="lineno">  776</span>&#160;</div><div class="line"><a name="l00777"></a><span class="lineno">  777</span>&#160;</div><div class="line"><a name="l00778"></a><span class="lineno">  778</span>&#160;    <span class="keyword">namespace </span>detail</div><div class="line"><a name="l00779"></a><span class="lineno">  779</span>&#160;    {</div><div class="line"><a name="l00780"></a><span class="lineno">  780</span>&#160;</div><div class="line"><a name="l00781"></a><span class="lineno">  781</span>&#160;</div><div class="line"><a name="l00782"></a><span class="lineno">  782</span>&#160;        <span class="comment">// &quot;Hard-coded&quot; constants used by this algorithm.</span></div><div class="line"><a name="l00783"></a><span class="lineno">  783</span>&#160;        <span class="comment">//</span></div><div class="line"><a name="l00784"></a><span class="lineno">  784</span>&#160;        <span class="comment">// For the time being, they are same whether single or double precision</span></div><div class="line"><a name="l00785"></a><span class="lineno">  785</span>&#160;        <span class="comment">// is used.</span></div><div class="line"><a name="l00786"></a><span class="lineno">  786</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00787"></a><span class="lineno"><a class="line" href="structpolar_1_1detail_1_1constants.html">  787</a></span>&#160;        <span class="keyword">struct </span><a class="code" href="structpolar_1_1detail_1_1constants.html">constants</a></div><div class="line"><a name="l00788"></a><span class="lineno">  788</span>&#160;        {</div><div class="line"><a name="l00789"></a><span class="lineno">  789</span>&#160;            <span class="comment">// Tolerance for determinant of matrix B.</span></div><div class="line"><a name="l00790"></a><span class="lineno">  790</span>&#160;            <span class="keyword">static</span> <span class="keyword">inline</span> TReal get_tau2();</div><div class="line"><a name="l00791"></a><span class="lineno">  791</span>&#160;            <span class="comment">// Tolerance for third of determinant of matrix B.</span></div><div class="line"><a name="l00792"></a><span class="lineno">  792</span>&#160;            <span class="keyword">static</span> <span class="keyword">inline</span> TReal get_tau1();</div><div class="line"><a name="l00793"></a><span class="lineno">  793</span>&#160;            <span class="comment">// Tolerance for Newton iterations.</span></div><div class="line"><a name="l00794"></a><span class="lineno">  794</span>&#160;            <span class="keyword">static</span> <span class="keyword">inline</span> TReal get_newton_tolerance();</div><div class="line"><a name="l00795"></a><span class="lineno">  795</span>&#160;            <span class="comment">// Threshold for sub-space iterations.</span></div><div class="line"><a name="l00796"></a><span class="lineno">  796</span>&#160;            <span class="keyword">static</span> <span class="keyword">inline</span> TReal get_subspace_threshold();</div><div class="line"><a name="l00797"></a><span class="lineno">  797</span>&#160;        };</div><div class="line"><a name="l00798"></a><span class="lineno">  798</span>&#160;</div><div class="line"><a name="l00799"></a><span class="lineno">  799</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00800"></a><span class="lineno">  800</span>&#160;        <span class="keyword">inline</span> TReal <a class="code" href="structpolar_1_1detail_1_1constants.html">constants&lt;TReal&gt;::get_tau2</a>()</div><div class="line"><a name="l00801"></a><span class="lineno">  801</span>&#160;        {</div><div class="line"><a name="l00802"></a><span class="lineno">  802</span>&#160;            <span class="keywordflow">return</span> <span class="keyword">static_cast&lt;</span>TReal<span class="keyword">&gt;</span>(1.0e-4);</div><div class="line"><a name="l00803"></a><span class="lineno">  803</span>&#160;        }</div><div class="line"><a name="l00804"></a><span class="lineno">  804</span>&#160;</div><div class="line"><a name="l00805"></a><span class="lineno">  805</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00806"></a><span class="lineno">  806</span>&#160;        <span class="keyword">inline</span> TReal constants&lt;TReal&gt;::get_tau1()</div><div class="line"><a name="l00807"></a><span class="lineno">  807</span>&#160;        {</div><div class="line"><a name="l00808"></a><span class="lineno">  808</span>&#160;            <span class="keywordflow">return</span> <span class="keyword">static_cast&lt;</span>TReal<span class="keyword">&gt;</span>(1.0e-4);</div><div class="line"><a name="l00809"></a><span class="lineno">  809</span>&#160;        }</div><div class="line"><a name="l00810"></a><span class="lineno">  810</span>&#160;</div><div class="line"><a name="l00811"></a><span class="lineno">  811</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00812"></a><span class="lineno">  812</span>&#160;        <span class="keyword">inline</span> TReal constants&lt;TReal&gt;::get_newton_tolerance()</div><div class="line"><a name="l00813"></a><span class="lineno">  813</span>&#160;        {</div><div class="line"><a name="l00814"></a><span class="lineno">  814</span>&#160;            <span class="comment">// The paper mentions 10^-15, but the Matlab implementation uses 10^-12.</span></div><div class="line"><a name="l00815"></a><span class="lineno">  815</span>&#160;            <span class="keywordflow">return</span> <span class="keyword">static_cast&lt;</span>TReal<span class="keyword">&gt;</span>(1.0e-12);</div><div class="line"><a name="l00816"></a><span class="lineno">  816</span>&#160;        }</div><div class="line"><a name="l00817"></a><span class="lineno">  817</span>&#160;</div><div class="line"><a name="l00818"></a><span class="lineno">  818</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00819"></a><span class="lineno">  819</span>&#160;        <span class="keyword">inline</span> TReal constants&lt;TReal&gt;::get_subspace_threshold()</div><div class="line"><a name="l00820"></a><span class="lineno">  820</span>&#160;        {</div><div class="line"><a name="l00821"></a><span class="lineno">  821</span>&#160;            <span class="comment">// This constant is used in the test:</span></div><div class="line"><a name="l00822"></a><span class="lineno">  822</span>&#160;            <span class="comment">// if log10 |u22| &gt; -7.18</span></div><div class="line"><a name="l00823"></a><span class="lineno">  823</span>&#160;            <span class="comment">// This implies that u22 &gt; 10^-7.18 ~= 6.607e-8</span></div><div class="line"><a name="l00824"></a><span class="lineno">  824</span>&#160;            <span class="comment">// However, this constant was determined using the machine error on</span></div><div class="line"><a name="l00825"></a><span class="lineno">  825</span>&#160;            <span class="comment">// floating-point representation, so it should probably be different</span></div><div class="line"><a name="l00826"></a><span class="lineno">  826</span>&#160;            <span class="comment">// between the single and double precision versions.</span></div><div class="line"><a name="l00827"></a><span class="lineno">  827</span>&#160;            <span class="keywordflow">return</span> <span class="keyword">static_cast&lt;</span>TReal<span class="keyword">&gt;</span>(6.607e-8);</div><div class="line"><a name="l00828"></a><span class="lineno">  828</span>&#160;        }</div><div class="line"><a name="l00829"></a><span class="lineno">  829</span>&#160;</div><div class="line"><a name="l00830"></a><span class="lineno">  830</span>&#160;</div><div class="line"><a name="l00831"></a><span class="lineno">  831</span>&#160;    }; <span class="comment">// End of namespace detail.</span></div><div class="line"><a name="l00832"></a><span class="lineno">  832</span>&#160;</div><div class="line"><a name="l00833"></a><span class="lineno">  833</span>&#160;</div><div class="line"><a name="l00834"></a><span class="lineno">  834</span>&#160;</div><div class="line"><a name="l00835"></a><span class="lineno">  835</span>&#160;</div><div class="line"><a name="l00836"></a><span class="lineno">  836</span>&#160;    <span class="comment">// Implementation of 3x3 polar decomposition based on:</span></div><div class="line"><a name="l00837"></a><span class="lineno">  837</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00838"></a><span class="lineno">  838</span>&#160;    <span class="comment">// &quot;An algorithm to compute the polar decomposition of a 3x3 matrix&quot;</span></div><div class="line"><a name="l00839"></a><span class="lineno">  839</span>&#160;    <span class="comment">// Nicholas J Higham and Vanni Noferini</span></div><div class="line"><a name="l00840"></a><span class="lineno">  840</span>&#160;    <span class="comment">// July 2015</span></div><div class="line"><a name="l00841"></a><span class="lineno">  841</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00842"></a><span class="lineno">  842</span>&#160;    <span class="comment">// The paper is available at:</span></div><div class="line"><a name="l00843"></a><span class="lineno">  843</span>&#160;    <span class="comment">// http://eprints.ma.man.ac.uk/2352/01/covered/MIMS_ep2015_66.pdf</span></div><div class="line"><a name="l00844"></a><span class="lineno">  844</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00845"></a><span class="lineno">  845</span>&#160;    <span class="comment">// It is now available at:</span></div><div class="line"><a name="l00846"></a><span class="lineno">  846</span>&#160;    <span class="comment">// http://link.springer.com/article/10.1007%2Fs11075-016-0098-7</span></div><div class="line"><a name="l00847"></a><span class="lineno">  847</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00848"></a><span class="lineno">  848</span>&#160;    <span class="comment">// This C++ implementation is also extensively based on the Matlab</span></div><div class="line"><a name="l00849"></a><span class="lineno">  849</span>&#160;    <span class="comment">// implementation of this algorithm available at:</span></div><div class="line"><a name="l00850"></a><span class="lineno">  850</span>&#160;    <span class="comment">// https://github.com/higham/polar-decomp-3by3</span></div><div class="line"><a name="l00851"></a><span class="lineno">  851</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00852"></a><span class="lineno">  852</span>&#160;    <span class="comment">// It is worth noting that Matlab uses a (row,column) matrix indexing,</span></div><div class="line"><a name="l00853"></a><span class="lineno">  853</span>&#160;    <span class="comment">// but this implementation uses (column,row) indexing.  Also, Matlab is</span></div><div class="line"><a name="l00854"></a><span class="lineno">  854</span>&#160;    <span class="comment">// 1-index based, but this implementation is 0-index based.</span></div><div class="line"><a name="l00855"></a><span class="lineno">  855</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00856"></a><span class="lineno">  856</span>&#160;    <span class="comment">// This implementation has very specific goals that drive implementation</span></div><div class="line"><a name="l00857"></a><span class="lineno">  857</span>&#160;    <span class="comment">// choices:</span></div><div class="line"><a name="l00858"></a><span class="lineno">  858</span>&#160;    <span class="comment">// - It tries to avoid dependencies to third-party libraries.  Therefore,</span></div><div class="line"><a name="l00859"></a><span class="lineno">  859</span>&#160;    <span class="comment">//   it reimplements basic operations (matrix operations such as multiplication,</span></div><div class="line"><a name="l00860"></a><span class="lineno">  860</span>&#160;    <span class="comment">//   transposition, etc.).  These are straight-forward to implement and</span></div><div class="line"><a name="l00861"></a><span class="lineno">  861</span>&#160;    <span class="comment">//   are kept separate to the actual algorithm so that using an actual</span></div><div class="line"><a name="l00862"></a><span class="lineno">  862</span>&#160;    <span class="comment">//   linear algebra library would make the implementation more straight-forward.</span></div><div class="line"><a name="l00863"></a><span class="lineno">  863</span>&#160;    <span class="comment">// - It tries to be as efficient as possible.  Therefore it potentially combines</span></div><div class="line"><a name="l00864"></a><span class="lineno">  864</span>&#160;    <span class="comment">//   multiple operations into one (for instance, combine matrix transposition</span></div><div class="line"><a name="l00865"></a><span class="lineno">  865</span>&#160;    <span class="comment">//   with multiplication) to minimize runtime cost.  While this might reduce</span></div><div class="line"><a name="l00866"></a><span class="lineno">  866</span>&#160;    <span class="comment">//   code simplicity, we favor runtime efficiency while trying to make those</span></div><div class="line"><a name="l00867"></a><span class="lineno">  867</span>&#160;    <span class="comment">//   optimizations as easy to read as possible.</span></div><div class="line"><a name="l00868"></a><span class="lineno">  868</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00869"></a><span class="lineno">  869</span>&#160;    <span class="comment">// It is also worth noting that this implementation relies on two major sources:</span></div><div class="line"><a name="l00870"></a><span class="lineno">  870</span>&#160;    <span class="comment">// - The algorithm as described in the paper</span></div><div class="line"><a name="l00871"></a><span class="lineno">  871</span>&#160;    <span class="comment">// - The algorithm as implemented in the Matlab implementation.</span></div><div class="line"><a name="l00872"></a><span class="lineno">  872</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00873"></a><span class="lineno">  873</span>&#160;    <span class="comment">// This implementation tries to highlight its references to both the paper and</span></div><div class="line"><a name="l00874"></a><span class="lineno">  874</span>&#160;    <span class="comment">// the source code.</span></div><div class="line"><a name="l00875"></a><span class="lineno">  875</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00876"></a><span class="lineno">  876</span>&#160;    <span class="comment">// The algorithms described in the paper are referred to using ### comments.</span></div><div class="line"><a name="l00877"></a><span class="lineno">  877</span>&#160;    <span class="comment">// For instance, specific lines such as the beginning of algorithm 3.5 will</span></div><div class="line"><a name="l00878"></a><span class="lineno">  878</span>&#160;    <span class="comment">// be highlighted:</span></div><div class="line"><a name="l00879"></a><span class="lineno">  879</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00880"></a><span class="lineno">  880</span>&#160;    <span class="comment">// ### Algorithm 3.5</span></div><div class="line"><a name="l00881"></a><span class="lineno">  881</span>&#160;    <span class="comment">//</span></div><div class="line"><a name="l00882"></a><span class="lineno">  882</span>&#160;    <span class="comment">// So that references to the paper are obvious.  References to the Matlab</span></div><div class="line"><a name="l00883"></a><span class="lineno">  883</span>&#160;    <span class="comment">// implementation of the algorithm will be more textual.</span></div><div class="line"><a name="l00884"></a><span class="lineno">  884</span>&#160;    <span class="keyword">namespace </span>detail</div><div class="line"><a name="l00885"></a><span class="lineno">  885</span>&#160;    {</div><div class="line"><a name="l00886"></a><span class="lineno">  886</span>&#160;</div><div class="line"><a name="l00887"></a><span class="lineno">  887</span>&#160;</div><div class="line"><a name="l00888"></a><span class="lineno">  888</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00889"></a><span class="lineno">  889</span>&#160;        <span class="keyword">inline</span> TReal run_algorithm_3_3(<span class="keyword">const</span> TReal absDetA, <span class="keyword">const</span> TReal detB);</div><div class="line"><a name="l00890"></a><span class="lineno">  890</span>&#160;</div><div class="line"><a name="l00891"></a><span class="lineno">  891</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00892"></a><span class="lineno">  892</span>&#160;        <span class="keyword">inline</span> TReal run_algorithm_3_4(<span class="keyword">const</span> TReal absDetA, <span class="keyword">const</span> TReal detB);</div><div class="line"><a name="l00893"></a><span class="lineno">  893</span>&#160;</div><div class="line"><a name="l00894"></a><span class="lineno">  894</span>&#160;        <span class="comment">// Implementation of algorithm 3.2.</span></div><div class="line"><a name="l00895"></a><span class="lineno">  895</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00896"></a><span class="lineno">  896</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> run_algorithm_3_2(</div><div class="line"><a name="l00897"></a><span class="lineno">  897</span>&#160;            vector&lt;TReal, 4&gt;&amp; v,</div><div class="line"><a name="l00898"></a><span class="lineno">  898</span>&#160;            vector&lt;int, 4&gt;&amp; p,</div><div class="line"><a name="l00899"></a><span class="lineno">  899</span>&#160;            <span class="keyword">const</span> matrix&lt;TReal, 3, 3&gt;&amp; A,</div><div class="line"><a name="l00900"></a><span class="lineno">  900</span>&#160;            <span class="keyword">const</span> matrix&lt;TReal, 4, 4&gt;&amp; B,</div><div class="line"><a name="l00901"></a><span class="lineno">  901</span>&#160;            <span class="keyword">const</span> TReal detB</div><div class="line"><a name="l00902"></a><span class="lineno">  902</span>&#160;            )</div><div class="line"><a name="l00903"></a><span class="lineno">  903</span>&#160;        {</div><div class="line"><a name="l00904"></a><span class="lineno">  904</span>&#160;            <span class="comment">// ### 1. Form B in R4? from A via (2.5).</span></div><div class="line"><a name="l00905"></a><span class="lineno">  905</span>&#160;</div><div class="line"><a name="l00906"></a><span class="lineno">  906</span>&#160;            <span class="comment">// ### 2. Compute b = det B from an LU factorization with partial pivoting.</span></div><div class="line"><a name="l00907"></a><span class="lineno">  907</span>&#160;            <span class="comment">// Already done.</span></div><div class="line"><a name="l00908"></a><span class="lineno">  908</span>&#160;            <span class="keyword">const</span> TReal b = detB;</div><div class="line"><a name="l00909"></a><span class="lineno">  909</span>&#160;</div><div class="line"><a name="l00910"></a><span class="lineno">  910</span>&#160;            <span class="comment">// ### 3. Compute d = det A from an LU factorization with partial pivoting.</span></div><div class="line"><a name="l00911"></a><span class="lineno">  911</span>&#160;            <span class="comment">// Sign of the determinant.</span></div><div class="line"><a name="l00912"></a><span class="lineno">  912</span>&#160;            TReal d;</div><div class="line"><a name="l00913"></a><span class="lineno">  913</span>&#160;            <span class="comment">// Determinant.</span></div><div class="line"><a name="l00914"></a><span class="lineno">  914</span>&#160;            TReal dd = compute_determinant_lu_partial(A, d);</div><div class="line"><a name="l00915"></a><span class="lineno">  915</span>&#160;            assert((d == 1) || (d == -1));</div><div class="line"><a name="l00916"></a><span class="lineno">  916</span>&#160;</div><div class="line"><a name="l00917"></a><span class="lineno">  917</span>&#160;            <span class="comment">// ### 4. if d &lt; 0, B = -B, d = -d, end</span></div><div class="line"><a name="l00918"></a><span class="lineno">  918</span>&#160;            <span class="comment">// We use the Bs matrix since we will need it anyways.  Bs matrix is formed from</span></div><div class="line"><a name="l00919"></a><span class="lineno">  919</span>&#160;            <span class="comment">// minus B matrix.</span></div><div class="line"><a name="l00920"></a><span class="lineno">  920</span>&#160;            matrix&lt;TReal, 4, 4&gt; Bs = B;</div><div class="line"><a name="l00921"></a><span class="lineno">  921</span>&#160;            multiply(Bs, -d);</div><div class="line"><a name="l00922"></a><span class="lineno">  922</span>&#160;            dd *= d;</div><div class="line"><a name="l00923"></a><span class="lineno">  923</span>&#160;</div><div class="line"><a name="l00924"></a><span class="lineno">  924</span>&#160;            <span class="comment">// ### 5. Estimate lambda1, a dominant eigenvalue of B, via Algorithm 3.3.</span></div><div class="line"><a name="l00925"></a><span class="lineno">  925</span>&#160;            <span class="keyword">const</span> TReal lambda1 = run_algorithm_3_3(dd, b);</div><div class="line"><a name="l00926"></a><span class="lineno">  926</span>&#160;</div><div class="line"><a name="l00927"></a><span class="lineno">  927</span>&#160;            <span class="comment">// ### 6. Bs = lambda1 I - B</span></div><div class="line"><a name="l00928"></a><span class="lineno">  928</span>&#160;            <span class="comment">// Bs already holds -B.</span></div><div class="line"><a name="l00929"></a><span class="lineno">  929</span>&#160;            Bs(0,0) += lambda1;</div><div class="line"><a name="l00930"></a><span class="lineno">  930</span>&#160;            Bs(1,1) += lambda1;</div><div class="line"><a name="l00931"></a><span class="lineno">  931</span>&#160;            Bs(2,2) += lambda1;</div><div class="line"><a name="l00932"></a><span class="lineno">  932</span>&#160;            Bs(3,3) += lambda1;</div><div class="line"><a name="l00933"></a><span class="lineno">  933</span>&#160;</div><div class="line"><a name="l00934"></a><span class="lineno">  934</span>&#160;            <span class="comment">// ### 7. Compute an LDLT factorization with diagonal pivoting, P^T Bs P = L D L^T.</span></div><div class="line"><a name="l00935"></a><span class="lineno">  935</span>&#160;            matrix&lt;TReal, 4, 4&gt; L;</div><div class="line"><a name="l00936"></a><span class="lineno">  936</span>&#160;            vector&lt;TReal, 4&gt; D;</div><div class="line"><a name="l00937"></a><span class="lineno">  937</span>&#160;            compute_ldlt_factorization_diagonal(L, D, p, Bs);</div><div class="line"><a name="l00938"></a><span class="lineno">  938</span>&#160;</div><div class="line"><a name="l00939"></a><span class="lineno">  939</span>&#160;            <span class="comment">// ### 8. v = PL^-T e4 / ||L^-T e4||2</span></div><div class="line"><a name="l00940"></a><span class="lineno">  940</span>&#160;            <span class="comment">// Normalization will be done in the common part.</span></div><div class="line"><a name="l00941"></a><span class="lineno">  941</span>&#160;            v(0) = L(0,1) * L(1,3) + L(0,2) * L(2,3) - L(0,1) * L(2,3) * L(1,2) - L(0,3);</div><div class="line"><a name="l00942"></a><span class="lineno">  942</span>&#160;            v(1) = L(2,3) * L(1,2) - L(1,3);</div><div class="line"><a name="l00943"></a><span class="lineno">  943</span>&#160;            v(2) = -L(2,3);</div><div class="line"><a name="l00944"></a><span class="lineno">  944</span>&#160;            v(3) = 1;</div><div class="line"><a name="l00945"></a><span class="lineno">  945</span>&#160;</div><div class="line"><a name="l00946"></a><span class="lineno">  946</span>&#160;            <span class="comment">// ### 9. Form the matrix Q using (2.7).</span></div><div class="line"><a name="l00947"></a><span class="lineno">  947</span>&#160;            <span class="comment">// ### 10. Compute the upper triangle of H = Q^T A and set the lower triangle equal to</span></div><div class="line"><a name="l00948"></a><span class="lineno">  948</span>&#160;            <span class="comment">//         the upper triangle.</span></div><div class="line"><a name="l00949"></a><span class="lineno">  949</span>&#160;            <span class="comment">// Both are done at the end of algorithm 3.5, along with v normalization.</span></div><div class="line"><a name="l00950"></a><span class="lineno">  950</span>&#160;        }</div><div class="line"><a name="l00951"></a><span class="lineno">  951</span>&#160;</div><div class="line"><a name="l00952"></a><span class="lineno">  952</span>&#160;</div><div class="line"><a name="l00953"></a><span class="lineno">  953</span>&#160;        <span class="comment">// Implementation of algorithm 3.3.</span></div><div class="line"><a name="l00954"></a><span class="lineno">  954</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l00955"></a><span class="lineno">  955</span>&#160;        <span class="keyword">inline</span> TReal run_algorithm_3_3(</div><div class="line"><a name="l00956"></a><span class="lineno">  956</span>&#160;            <span class="keyword">const</span> TReal absDetA,</div><div class="line"><a name="l00957"></a><span class="lineno">  957</span>&#160;            <span class="keyword">const</span> TReal detB</div><div class="line"><a name="l00958"></a><span class="lineno">  958</span>&#160;            )</div><div class="line"><a name="l00959"></a><span class="lineno">  959</span>&#160;        {</div><div class="line"><a name="l00960"></a><span class="lineno">  960</span>&#160;            TReal lambda1;</div><div class="line"><a name="l00961"></a><span class="lineno">  961</span>&#160;</div><div class="line"><a name="l00962"></a><span class="lineno">  962</span>&#160;            <span class="keyword">const</span> TReal&amp; dd = absDetA;</div><div class="line"><a name="l00963"></a><span class="lineno">  963</span>&#160;            <span class="keyword">const</span> TReal&amp; b = detB;</div><div class="line"><a name="l00964"></a><span class="lineno">  964</span>&#160;</div><div class="line"><a name="l00965"></a><span class="lineno">  965</span>&#160;            <span class="comment">// ### 1. tau1 = 10^4 % Tolerance.</span></div><div class="line"><a name="l00966"></a><span class="lineno">  966</span>&#160;            <span class="keyword">static</span> <span class="keyword">const</span> TReal kTau1 = constants&lt;TReal&gt;::get_tau1();</div><div class="line"><a name="l00967"></a><span class="lineno">  967</span>&#160;</div><div class="line"><a name="l00968"></a><span class="lineno">  968</span>&#160;            <span class="comment">// ### 2. if b + 1/3 &gt; 1</span></div><div class="line"><a name="l00969"></a><span class="lineno">  969</span>&#160;            <span class="keywordflow">if</span> (b &gt; kTau1 - 1 / static_cast&lt;TReal&gt;(3))</div><div class="line"><a name="l00970"></a><span class="lineno">  970</span>&#160;            {</div><div class="line"><a name="l00971"></a><span class="lineno">  971</span>&#160;                <span class="comment">// ### 3. c = 8d</span></div><div class="line"><a name="l00972"></a><span class="lineno">  972</span>&#160;                <span class="keyword">const</span> TReal c = 8 * dd;</div><div class="line"><a name="l00973"></a><span class="lineno">  973</span>&#160;                <span class="comment">// ### 4. delta0 = 1 + 3b</span></div><div class="line"><a name="l00974"></a><span class="lineno">  974</span>&#160;                <span class="keyword">const</span> TReal delta0 = 1 + 3 * b;</div><div class="line"><a name="l00975"></a><span class="lineno">  975</span>&#160;                <span class="comment">// ### 5. delta1 = -1 + (27/16)c^2 + 9b</span></div><div class="line"><a name="l00976"></a><span class="lineno">  976</span>&#160;                <span class="keyword">const</span> TReal delta1 = -1 + (27 / <span class="keyword">static_cast&lt;</span>TReal<span class="keyword">&gt;</span>(16)) * c * c + 9 * b;</div><div class="line"><a name="l00977"></a><span class="lineno">  977</span>&#160;                <span class="comment">// ### 6. phi = delta1/delta0^(3/2)</span></div><div class="line"><a name="l00978"></a><span class="lineno">  978</span>&#160;                TReal phi = delta1 / (delta0 * math_utils&lt;TReal&gt;::sqrt(delta0));</div><div class="line"><a name="l00979"></a><span class="lineno">  979</span>&#160;                <span class="comment">// This was not in the original algorithm, but clamp to [-1,1] in case of rounding errors.</span></div><div class="line"><a name="l00980"></a><span class="lineno">  980</span>&#160;                phi = math_utils&lt;TReal&gt;::clamp(phi, -1, 1);</div><div class="line"><a name="l00981"></a><span class="lineno">  981</span>&#160;                <span class="comment">// ### 7. z = (4/3)(1 + delta0^(1/2)cos(arccos(alpha)/3))</span></div><div class="line"><a name="l00982"></a><span class="lineno">  982</span>&#160;                <span class="keyword">const</span> TReal z = (4 / <span class="keyword">static_cast&lt;</span>TReal<span class="keyword">&gt;</span>(3)) * (1 + math_utils&lt;TReal&gt;::sqrt(delta0) * math_utils&lt;TReal&gt;::cos(math_utils&lt;TReal&gt;::acos(phi) / 3));</div><div class="line"><a name="l00983"></a><span class="lineno">  983</span>&#160;                <span class="comment">// ### 8. s = z^0.5/2</span></div><div class="line"><a name="l00984"></a><span class="lineno">  984</span>&#160;                <span class="keyword">const</span> TReal s = math_utils&lt;TReal&gt;::sqrt(z) / 2;</div><div class="line"><a name="l00985"></a><span class="lineno">  985</span>&#160;                <span class="comment">// ### 9. lambda1 = s + (max(0, 4 - z + c/s))^(1/2)/2.</span></div><div class="line"><a name="l00986"></a><span class="lineno">  986</span>&#160;                lambda1 = s;</div><div class="line"><a name="l00987"></a><span class="lineno">  987</span>&#160;                <span class="keyword">const</span> TReal temp = 4 - z + c / s;</div><div class="line"><a name="l00988"></a><span class="lineno">  988</span>&#160;                <span class="keywordflow">if</span> (temp &gt; 0)</div><div class="line"><a name="l00989"></a><span class="lineno">  989</span>&#160;                    lambda1 += math_utils&lt;TReal&gt;::sqrt(temp) / 2;</div><div class="line"><a name="l00990"></a><span class="lineno">  990</span>&#160;            }</div><div class="line"><a name="l00991"></a><span class="lineno">  991</span>&#160;            <span class="comment">// ### 10. else</span></div><div class="line"><a name="l00992"></a><span class="lineno">  992</span>&#160;            <span class="keywordflow">else</span></div><div class="line"><a name="l00993"></a><span class="lineno">  993</span>&#160;            {</div><div class="line"><a name="l00994"></a><span class="lineno">  994</span>&#160;                <span class="comment">// ### 11. Use Newton&#39;s method (Algorithm 3.4) to approximate</span></div><div class="line"><a name="l00995"></a><span class="lineno">  995</span>&#160;                lambda1 = run_algorithm_3_4(dd, b);</div><div class="line"><a name="l00996"></a><span class="lineno">  996</span>&#160;            }</div><div class="line"><a name="l00997"></a><span class="lineno">  997</span>&#160;            <span class="comment">// ### 12. end</span></div><div class="line"><a name="l00998"></a><span class="lineno">  998</span>&#160;</div><div class="line"><a name="l00999"></a><span class="lineno">  999</span>&#160;            <span class="keywordflow">return</span> lambda1;</div><div class="line"><a name="l01000"></a><span class="lineno"> 1000</span>&#160;        }</div><div class="line"><a name="l01001"></a><span class="lineno"> 1001</span>&#160;</div><div class="line"><a name="l01002"></a><span class="lineno"> 1002</span>&#160;</div><div class="line"><a name="l01003"></a><span class="lineno"> 1003</span>&#160;        <span class="comment">// Implementation of algorithm 3.4.</span></div><div class="line"><a name="l01004"></a><span class="lineno"> 1004</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l01005"></a><span class="lineno"> 1005</span>&#160;        <span class="keyword">inline</span> TReal run_algorithm_3_4(</div><div class="line"><a name="l01006"></a><span class="lineno"> 1006</span>&#160;            <span class="keyword">const</span> TReal absDetA,</div><div class="line"><a name="l01007"></a><span class="lineno"> 1007</span>&#160;            <span class="keyword">const</span> TReal detB</div><div class="line"><a name="l01008"></a><span class="lineno"> 1008</span>&#160;            )</div><div class="line"><a name="l01009"></a><span class="lineno"> 1009</span>&#160;        {</div><div class="line"><a name="l01010"></a><span class="lineno"> 1010</span>&#160;            <span class="keyword">const</span> TReal&amp; dd = absDetA;</div><div class="line"><a name="l01011"></a><span class="lineno"> 1011</span>&#160;            <span class="keyword">const</span> TReal&amp; b = detB;</div><div class="line"><a name="l01012"></a><span class="lineno"> 1012</span>&#160;</div><div class="line"><a name="l01013"></a><span class="lineno"> 1013</span>&#160;            <span class="comment">// ### 1. x = sqrt(3)</span></div><div class="line"><a name="l01014"></a><span class="lineno"> 1014</span>&#160;            TReal x = math_utils&lt;TReal&gt;::sqrt(3);</div><div class="line"><a name="l01015"></a><span class="lineno"> 1015</span>&#160;            <span class="comment">// ### 2. xold = 3</span></div><div class="line"><a name="l01016"></a><span class="lineno"> 1016</span>&#160;            TReal xold = 3;</div><div class="line"><a name="l01017"></a><span class="lineno"> 1017</span>&#160;            <span class="comment">// ### 3. while xold - x &gt; 10^-15</span></div><div class="line"><a name="l01018"></a><span class="lineno"> 1018</span>&#160;            <span class="keyword">static</span> <span class="keyword">const</span> TReal kNewtonTolerance = constants&lt;TReal&gt;::get_newton_tolerance();</div><div class="line"><a name="l01019"></a><span class="lineno"> 1019</span>&#160;            <span class="keywordflow">while</span> (xold - x &gt; kNewtonTolerance)</div><div class="line"><a name="l01020"></a><span class="lineno"> 1020</span>&#160;            {</div><div class="line"><a name="l01021"></a><span class="lineno"> 1021</span>&#160;                <span class="comment">// ### 4. xold = x</span></div><div class="line"><a name="l01022"></a><span class="lineno"> 1022</span>&#160;                xold = x;</div><div class="line"><a name="l01023"></a><span class="lineno"> 1023</span>&#160;                <span class="comment">// ### 5. Evaluate p = p(x) = det(xI - B) by Horner&#39;s method.</span></div><div class="line"><a name="l01024"></a><span class="lineno"> 1024</span>&#160;                <span class="keyword">const</span> TReal c = 8 * dd;</div><div class="line"><a name="l01025"></a><span class="lineno"> 1025</span>&#160;                <span class="keyword">const</span> TReal px = x * (x * (x * x - 2) - c) + b;</div><div class="line"><a name="l01026"></a><span class="lineno"> 1026</span>&#160;                <span class="comment">// ### 6. Evaluate pd = p0(x) by Horner&#39;s method.</span></div><div class="line"><a name="l01027"></a><span class="lineno"> 1027</span>&#160;                <span class="keyword">const</span> TReal dpx = x * (4 * x * x - 4) - c;</div><div class="line"><a name="l01028"></a><span class="lineno"> 1028</span>&#160;                <span class="comment">// ### 7. x = x - p/pd</span></div><div class="line"><a name="l01029"></a><span class="lineno"> 1029</span>&#160;                x = x - px / dpx;</div><div class="line"><a name="l01030"></a><span class="lineno"> 1030</span>&#160;            }</div><div class="line"><a name="l01031"></a><span class="lineno"> 1031</span>&#160;            <span class="comment">// ### 8. end</span></div><div class="line"><a name="l01032"></a><span class="lineno"> 1032</span>&#160;            <span class="keyword">const</span> TReal lambda1 = x;</div><div class="line"><a name="l01033"></a><span class="lineno"> 1033</span>&#160;</div><div class="line"><a name="l01034"></a><span class="lineno"> 1034</span>&#160;            <span class="keywordflow">return</span> lambda1;</div><div class="line"><a name="l01035"></a><span class="lineno"> 1035</span>&#160;        }</div><div class="line"><a name="l01036"></a><span class="lineno"> 1036</span>&#160;</div><div class="line"><a name="l01037"></a><span class="lineno"> 1037</span>&#160;</div><div class="line"><a name="l01038"></a><span class="lineno"> 1038</span>&#160;        <span class="comment">// Implementation of algorithm 3.5.</span></div><div class="line"><a name="l01039"></a><span class="lineno"> 1039</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> TReal&gt;</div><div class="line"><a name="l01040"></a><span class="lineno"> 1040</span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> run_algorithm_3_5(</div><div class="line"><a name="l01041"></a><span class="lineno"> 1041</span>&#160;            matrix&lt;TReal, 3, 3&gt;&amp; paramQ,</div><div class="line"><a name="l01042"></a><span class="lineno"> 1042</span>&#160;            matrix&lt;TReal, 3, 3&gt;&amp; paramH,</div><div class="line"><a name="l01043"></a><span class="lineno"> 1043</span>&#160;            <span class="keyword">const</span> matrix&lt;TReal, 3, 3&gt;&amp; paramA</div><div class="line"><a name="l01044"></a><span class="lineno"> 1044</span>&#160;            )</div><div class="line"><a name="l01045"></a><span class="lineno"> 1045</span>&#160;        {</div><div class="line"><a name="l01046"></a><span class="lineno"> 1046</span>&#160;            <span class="comment">// First make sure the input matrix is normalized.</span></div><div class="line"><a name="l01047"></a><span class="lineno"> 1047</span>&#160;            matrix&lt;TReal, 3, 3&gt; A = paramA;</div><div class="line"><a name="l01048"></a><span class="lineno"> 1048</span>&#160;            normalize(A);</div><div class="line"><a name="l01049"></a><span class="lineno"> 1049</span>&#160;</div><div class="line"><a name="l01050"></a><span class="lineno"> 1050</span>&#160;            <span class="comment">// ### Algorithm 3.5</span></div><div class="line"><a name="l01051"></a><span class="lineno"> 1051</span>&#160;</div><div class="line"><a name="l01052"></a><span class="lineno"> 1052</span>&#160;            <span class="comment">// ### 1. tau2 = 10^-4 % Tolerance.</span></div><div class="line"><a name="l01053"></a><span class="lineno"> 1053</span>&#160;            <span class="keyword">static</span> <span class="keyword">const</span> TReal kTau2 = constants&lt;TReal&gt;::get_tau2();</div><div class="line"><a name="l01054"></a><span class="lineno"> 1054</span>&#160;</div><div class="line"><a name="l01055"></a><span class="lineno"> 1055</span>&#160;            <span class="comment">// ### 2. Form B in R4? from A via (2.5).</span></div><div class="line"><a name="l01056"></a><span class="lineno"> 1056</span>&#160;            matrix&lt;TReal, 4, 4&gt; B;</div><div class="line"><a name="l01057"></a><span class="lineno"> 1057</span>&#160;            <span class="comment">// Computation of the matrix as described in the paper&#39;s formula.</span></div><div class="line"><a name="l01058"></a><span class="lineno"> 1058</span>&#160;            <span class="comment">// Note: In the author&#39;s Matlab implementation, the computation</span></div><div class="line"><a name="l01059"></a><span class="lineno"> 1059</span>&#160;            <span class="comment">// is slightly different.  It first computes the trace as the</span></div><div class="line"><a name="l01060"></a><span class="lineno"> 1060</span>&#160;            <span class="comment">// sum of the diagonal and then computes the diagonal elements</span></div><div class="line"><a name="l01061"></a><span class="lineno"> 1061</span>&#160;            <span class="comment">// of B derived from this.  This can create numerical differences</span></div><div class="line"><a name="l01062"></a><span class="lineno"> 1062</span>&#160;            <span class="comment">// for which the algorithm should be tolerant, but depending on</span></div><div class="line"><a name="l01063"></a><span class="lineno"> 1063</span>&#160;            <span class="comment">// whether single or double precision is used, different paths</span></div><div class="line"><a name="l01064"></a><span class="lineno"> 1064</span>&#160;            <span class="comment">// in the algorithm might be used.  However, they should all yield</span></div><div class="line"><a name="l01065"></a><span class="lineno"> 1065</span>&#160;            <span class="comment">// satisfactory solutions as the algorithm should be numerically stable</span></div><div class="line"><a name="l01066"></a><span class="lineno"> 1066</span>&#160;            <span class="comment">// in both cases.</span></div><div class="line"><a name="l01067"></a><span class="lineno"> 1067</span>&#160;            <span class="comment">//</span></div><div class="line"><a name="l01068"></a><span class="lineno"> 1068</span>&#160;            <span class="comment">// OPTME: B matrix is symmetric, we could store it in a different way.</span></div><div class="line"><a name="l01069"></a><span class="lineno"> 1069</span>&#160;            B(0,0) = A(0,0) + A(1,1) + A(2,2);</div><div class="line"><a name="l01070"></a><span class="lineno"> 1070</span>&#160;            B(1,1) = A(0,0) - A(1,1) - A(2,2);</div><div class="line"><a name="l01071"></a><span class="lineno"> 1071</span>&#160;            B(2,2) = A(1,1) - A(0,0) - A(2,2);</div><div class="line"><a name="l01072"></a><span class="lineno"> 1072</span>&#160;            B(3,3) = A(2,2) - A(0,0) - A(1,1);</div><div class="line"><a name="l01073"></a><span class="lineno"> 1073</span>&#160;            B(0,1) = B(1,0) = A(2,1) - A(1,2);</div><div class="line"><a name="l01074"></a><span class="lineno"> 1074</span>&#160;            B(1,2) = B(2,1) = A(1,0) + A(0,1);</div><div class="line"><a name="l01075"></a><span class="lineno"> 1075</span>&#160;            B(2,3) = B(3,2) = A(2,1) + A(1,2);</div><div class="line"><a name="l01076"></a><span class="lineno"> 1076</span>&#160;            B(0,2) = B(2,0) = A(0,2) - A(2,0);</div><div class="line"><a name="l01077"></a><span class="lineno"> 1077</span>&#160;            B(1,3) = B(3,1) = A(2,0) + A(0,2);</div><div class="line"><a name="l01078"></a><span class="lineno"> 1078</span>&#160;            B(0,3) = B(3,0) = A(1,0) - A(0,1);</div><div class="line"><a name="l01079"></a><span class="lineno"> 1079</span>&#160;</div><div class="line"><a name="l01080"></a><span class="lineno"> 1080</span>&#160;            <span class="comment">// ### 3. Compute b = det B from an LU factorization with partial pivoting.</span></div><div class="line"><a name="l01081"></a><span class="lineno"> 1081</span>&#160;            <span class="comment">// It actually computes B determinant from A matrix is B is a function of A.</span></div><div class="line"><a name="l01082"></a><span class="lineno"> 1082</span>&#160;            <span class="keyword">const</span> TReal b = compute_b_determinant_from_a_matrix_lu_partial(A);</div><div class="line"><a name="l01083"></a><span class="lineno"> 1083</span>&#160;</div><div class="line"><a name="l01084"></a><span class="lineno"> 1084</span>&#160;            vector&lt;TReal, 4&gt; v;</div><div class="line"><a name="l01085"></a><span class="lineno"> 1085</span>&#160;            vector&lt;int, 4&gt; p;</div><div class="line"><a name="l01086"></a><span class="lineno"> 1086</span>&#160;</div><div class="line"><a name="l01087"></a><span class="lineno"> 1087</span>&#160;            <span class="comment">// ### 4. if b &lt; 1 - tau2</span></div><div class="line"><a name="l01088"></a><span class="lineno"> 1088</span>&#160;            <span class="keywordflow">if</span> (b &lt; 1 - kTau2)</div><div class="line"><a name="l01089"></a><span class="lineno"> 1089</span>&#160;            {</div><div class="line"><a name="l01090"></a><span class="lineno"> 1090</span>&#160;                <span class="comment">// ### % Dominant eigenvalue of B is well separated.</span></div><div class="line"><a name="l01091"></a><span class="lineno"> 1091</span>&#160;                <span class="comment">// ### 5. Call Algorithm 3.2.</span></div><div class="line"><a name="l01092"></a><span class="lineno"> 1092</span>&#160;                run_algorithm_3_2(v, p, A, B, b);</div><div class="line"><a name="l01093"></a><span class="lineno"> 1093</span>&#160;            }</div><div class="line"><a name="l01094"></a><span class="lineno"> 1094</span>&#160;            <span class="comment">// ### 6. else</span></div><div class="line"><a name="l01095"></a><span class="lineno"> 1095</span>&#160;            <span class="keywordflow">else</span></div><div class="line"><a name="l01096"></a><span class="lineno"> 1096</span>&#160;            {</div><div class="line"><a name="l01097"></a><span class="lineno"> 1097</span>&#160;                <span class="comment">// ### 7. Compute d = detA using an LU factorization with complete pivoting.</span></div><div class="line"><a name="l01098"></a><span class="lineno"> 1098</span>&#160;                <span class="comment">// Also keep the second U value of the factorization.</span></div><div class="line"><a name="l01099"></a><span class="lineno"> 1099</span>&#160;                TReal u22;</div><div class="line"><a name="l01100"></a><span class="lineno"> 1100</span>&#160;                <span class="comment">// Sign of the determinant.</span></div><div class="line"><a name="l01101"></a><span class="lineno"> 1101</span>&#160;                TReal d;</div><div class="line"><a name="l01102"></a><span class="lineno"> 1102</span>&#160;                <span class="comment">// Determinant.</span></div><div class="line"><a name="l01103"></a><span class="lineno"> 1103</span>&#160;                TReal dd = compute_determinant_lu_complete(A, d, u22);</div><div class="line"><a name="l01104"></a><span class="lineno"> 1104</span>&#160;                assert((d == 1) || (d == -1));</div><div class="line"><a name="l01105"></a><span class="lineno"> 1105</span>&#160;</div><div class="line"><a name="l01106"></a><span class="lineno"> 1106</span>&#160;                <span class="comment">// ### 8. If d &lt; 0, B = -B, end</span></div><div class="line"><a name="l01107"></a><span class="lineno"> 1107</span>&#160;                <span class="comment">// We use the Bs matrix since we will need it anyways.  Bs matrix is formed from</span></div><div class="line"><a name="l01108"></a><span class="lineno"> 1108</span>&#160;                <span class="comment">// minus B matrix.</span></div><div class="line"><a name="l01109"></a><span class="lineno"> 1109</span>&#160;                matrix&lt;TReal, 4, 4&gt; Bs = B;</div><div class="line"><a name="l01110"></a><span class="lineno"> 1110</span>&#160;                multiply(Bs, -d);</div><div class="line"><a name="l01111"></a><span class="lineno"> 1111</span>&#160;                dd *= d;</div><div class="line"><a name="l01112"></a><span class="lineno"> 1112</span>&#160;</div><div class="line"><a name="l01113"></a><span class="lineno"> 1113</span>&#160;                <span class="comment">// ### 9. Estimate lambda1 using Algorithm 3.3.</span></div><div class="line"><a name="l01114"></a><span class="lineno"> 1114</span>&#160;                <span class="keyword">const</span> TReal lambda1 = run_algorithm_3_3(dd, b);</div><div class="line"><a name="l01115"></a><span class="lineno"> 1115</span>&#160;</div><div class="line"><a name="l01116"></a><span class="lineno"> 1116</span>&#160;                <span class="comment">// ### 10. Bs = lambda1 I - B</span></div><div class="line"><a name="l01117"></a><span class="lineno"> 1117</span>&#160;                <span class="comment">// Bs already holds -B.</span></div><div class="line"><a name="l01118"></a><span class="lineno"> 1118</span>&#160;                Bs(0, 0) += lambda1;</div><div class="line"><a name="l01119"></a><span class="lineno"> 1119</span>&#160;                Bs(1, 1) += lambda1;</div><div class="line"><a name="l01120"></a><span class="lineno"> 1120</span>&#160;                Bs(2, 2) += lambda1;</div><div class="line"><a name="l01121"></a><span class="lineno"> 1121</span>&#160;                Bs(3, 3) += lambda1;</div><div class="line"><a name="l01122"></a><span class="lineno"> 1122</span>&#160;</div><div class="line"><a name="l01123"></a><span class="lineno"> 1123</span>&#160;                <span class="comment">// Compute Bs = LDL^T by block LDL^T factorization with Bunch-Parlett pivoting</span></div><div class="line"><a name="l01124"></a><span class="lineno"> 1124</span>&#160;                <span class="comment">// This will be used in both following cases.</span></div><div class="line"><a name="l01125"></a><span class="lineno"> 1125</span>&#160;                matrix&lt;TReal, 4, 4&gt; L;</div><div class="line"><a name="l01126"></a><span class="lineno"> 1126</span>&#160;                matrix&lt;TReal, 4, 4&gt; D;</div><div class="line"><a name="l01127"></a><span class="lineno"> 1127</span>&#160;                compute_ldlt_factorization_bunch_parlett(L, D, p, Bs);</div><div class="line"><a name="l01128"></a><span class="lineno"> 1128</span>&#160;                assert(D(2,3) == D(3,2));</div><div class="line"><a name="l01129"></a><span class="lineno"> 1129</span>&#160;</div><div class="line"><a name="l01130"></a><span class="lineno"> 1130</span>&#160;                <span class="keyword">const</span> TReal DD = D(2,2) * D(3,3) - D(3,2) * D(3,2);</div><div class="line"><a name="l01131"></a><span class="lineno"> 1131</span>&#160;                <span class="keywordflow">if</span> (DD == 0)</div><div class="line"><a name="l01132"></a><span class="lineno"> 1132</span>&#160;                {</div><div class="line"><a name="l01133"></a><span class="lineno"> 1133</span>&#160;                    <span class="comment">// Treat this case specially.  It is not really mentioned in the paper&#39;s algorithm,</span></div><div class="line"><a name="l01134"></a><span class="lineno"> 1134</span>&#160;                    <span class="comment">// but it is part of the Matlab implementation.</span></div><div class="line"><a name="l01135"></a><span class="lineno"> 1135</span>&#160;                    <span class="keyword">const</span> <span class="keywordtype">bool</span> allZero = (D(2,2) == 0) &amp;&amp; (D(3,3) == 0) &amp;&amp; (D(3,2) == 0);</div><div class="line"><a name="l01136"></a><span class="lineno"> 1136</span>&#160;                    <span class="keywordflow">if</span> (allZero)</div><div class="line"><a name="l01137"></a><span class="lineno"> 1137</span>&#160;                    {</div><div class="line"><a name="l01138"></a><span class="lineno"> 1138</span>&#160;                        <span class="comment">// This is the equivalent of choosing a null space of (0,1) and do</span></div><div class="line"><a name="l01139"></a><span class="lineno"> 1139</span>&#160;                        <span class="comment">// the same calculation as the other case.</span></div><div class="line"><a name="l01140"></a><span class="lineno"> 1140</span>&#160;                        v(0) = L(0,1) * L(1,3) - L(0,3);</div><div class="line"><a name="l01141"></a><span class="lineno"> 1141</span>&#160;                        v(1) = -L(1,3);</div><div class="line"><a name="l01142"></a><span class="lineno"> 1142</span>&#160;                        v(2) = 0;</div><div class="line"><a name="l01143"></a><span class="lineno"> 1143</span>&#160;                        v(3) = 1;</div><div class="line"><a name="l01144"></a><span class="lineno"> 1144</span>&#160;                    }</div><div class="line"><a name="l01145"></a><span class="lineno"> 1145</span>&#160;                    <span class="keywordflow">else</span></div><div class="line"><a name="l01146"></a><span class="lineno"> 1146</span>&#160;                    {</div><div class="line"><a name="l01147"></a><span class="lineno"> 1147</span>&#160;                        <span class="comment">// ANSME: A more robust way might be to get into this case for determinant close to 0,</span></div><div class="line"><a name="l01148"></a><span class="lineno"> 1148</span>&#160;                        <span class="comment">// instead of exactly equal to zero, and then use something more robust such as taking</span></div><div class="line"><a name="l01149"></a><span class="lineno"> 1149</span>&#160;                        <span class="comment">// the vector associated with the smallest singular value (0 if there is actually a</span></div><div class="line"><a name="l01150"></a><span class="lineno"> 1150</span>&#160;                        <span class="comment">// null space), which could probably be computed efficiently for 2x2 matrices.</span></div><div class="line"><a name="l01151"></a><span class="lineno"> 1151</span>&#160;                        <span class="keyword">const</span> vector&lt;TReal, 2&gt; nullSpace = compute_null_space(D(2,2), D(2,3), D(3,3));</div><div class="line"><a name="l01152"></a><span class="lineno"> 1152</span>&#160;</div><div class="line"><a name="l01153"></a><span class="lineno"> 1153</span>&#160;                        <span class="comment">// Since L is diagonal, we can solve v = L^-T * [0 0 a b]^T.</span></div><div class="line"><a name="l01154"></a><span class="lineno"> 1154</span>&#160;                        <span class="comment">// We also know, from compute_ldlt_factorization_bunch_parlett, that L(2,3) is 0.</span></div><div class="line"><a name="l01155"></a><span class="lineno"> 1155</span>&#160;                        <span class="comment">// So v can be computed symbolically by WolframAlpha by running the following query:</span></div><div class="line"><a name="l01156"></a><span class="lineno"> 1156</span>&#160;                        <span class="comment">// Inverse[Transpose[{{1,0,0,0},{l_12,1,0,0},{l_13,l_23,1,0},{l_14,l_24,0,1}}]] * {{0},{0},{a},{b}}</span></div><div class="line"><a name="l01157"></a><span class="lineno"> 1157</span>&#160;                        <span class="comment">//</span></div><div class="line"><a name="l01158"></a><span class="lineno"> 1158</span>&#160;                        <span class="comment">// We have L^-T = | 1    -L(0,1)    L(0,1)*L(1,2) - L(0,2)    -L(0,3) + L(0,2)*L(2,3) + L(0,1)*(L(1,3) - L(1,2)*L(2,3)) |</span></div><div class="line"><a name="l01159"></a><span class="lineno"> 1159</span>&#160;                        <span class="comment">//                | 0    1          -L(1,2)                   L(1,2)*L(2,3) - L(1,3)                                    |</span></div><div class="line"><a name="l01160"></a><span class="lineno"> 1160</span>&#160;                        <span class="comment">//                | 0    0          1                         -L(2,3)                                                   |</span></div><div class="line"><a name="l01161"></a><span class="lineno"> 1161</span>&#160;                        <span class="comment">//                | 0    0          0                         1                                                         |</span></div><div class="line"><a name="l01162"></a><span class="lineno"> 1162</span>&#160;                        <span class="comment">//</span></div><div class="line"><a name="l01163"></a><span class="lineno"> 1163</span>&#160;                        <span class="comment">// Using L(2,3) == 0 we get:</span></div><div class="line"><a name="l01164"></a><span class="lineno"> 1164</span>&#160;                        <span class="comment">//</span></div><div class="line"><a name="l01165"></a><span class="lineno"> 1165</span>&#160;                        <span class="comment">// We have L^-T = | 1    -L(0,1)    L(0,1)*L(1,2) - L(0,2)    L(0,1)*L(1,3) -L(0,3) |</span></div><div class="line"><a name="l01166"></a><span class="lineno"> 1166</span>&#160;                        <span class="comment">//                | 0    1          -L(1,2)                   -L(1,3)               |</span></div><div class="line"><a name="l01167"></a><span class="lineno"> 1167</span>&#160;                        <span class="comment">//                | 0    0          1                         0                     |</span></div><div class="line"><a name="l01168"></a><span class="lineno"> 1168</span>&#160;                        <span class="comment">//                | 0    0          0                         1                     |</span></div><div class="line"><a name="l01169"></a><span class="lineno"> 1169</span>&#160;                        <span class="comment">//</span></div><div class="line"><a name="l01170"></a><span class="lineno"> 1170</span>&#160;                        <span class="comment">// Multiplied by [0 0 a b]^T:</span></div><div class="line"><a name="l01171"></a><span class="lineno"> 1171</span>&#160;                        <span class="comment">//</span></div><div class="line"><a name="l01172"></a><span class="lineno"> 1172</span>&#160;                        <span class="comment">// | a * (L(0,1)*L(1,2) - L(0,2)) + b * (L(0,1)*L(1,3) - L(0,3)) |</span></div><div class="line"><a name="l01173"></a><span class="lineno"> 1173</span>&#160;                        <span class="comment">// | -a * L(1,2) - b * L(1,3)                                    |</span></div><div class="line"><a name="l01174"></a><span class="lineno"> 1174</span>&#160;                        <span class="comment">// | a                                                           |</span></div><div class="line"><a name="l01175"></a><span class="lineno"> 1175</span>&#160;                        <span class="comment">// | b                                                           |</span></div><div class="line"><a name="l01176"></a><span class="lineno"> 1176</span>&#160;</div><div class="line"><a name="l01177"></a><span class="lineno"> 1177</span>&#160;                        v(0) = nullSpace(0) * (L(0,1) * L(1,2) - L(0,2)) + nullSpace(1) * (L(0,1) * L(1,3) - L(0,3));</div><div class="line"><a name="l01178"></a><span class="lineno"> 1178</span>&#160;                        v(1) = -nullSpace(0) * L(1,2) - nullSpace(1) * L(1,3);</div><div class="line"><a name="l01179"></a><span class="lineno"> 1179</span>&#160;                        v(2) = nullSpace(0);</div><div class="line"><a name="l01180"></a><span class="lineno"> 1180</span>&#160;                        v(3) = nullSpace(1);</div><div class="line"><a name="l01181"></a><span class="lineno"> 1181</span>&#160;                    }</div><div class="line"><a name="l01182"></a><span class="lineno"> 1182</span>&#160;                }</div><div class="line"><a name="l01183"></a><span class="lineno"> 1183</span>&#160;                <span class="keywordflow">else</span></div><div class="line"><a name="l01184"></a><span class="lineno"> 1184</span>&#160;                {</div><div class="line"><a name="l01185"></a><span class="lineno"> 1185</span>&#160;                    <span class="comment">// Compute inverse of L.</span></div><div class="line"><a name="l01186"></a><span class="lineno"> 1186</span>&#160;                    <span class="comment">// See above for explanation of L^-1 computation (and assumptions about which values are 0 and 1).</span></div><div class="line"><a name="l01187"></a><span class="lineno"> 1187</span>&#160;                    <span class="keyword">const</span> TReal IL01 = -L(0,1);</div><div class="line"><a name="l01188"></a><span class="lineno"> 1188</span>&#160;                    <span class="keyword">const</span> TReal IL02 = L(0,1) * L(1,2) - L(0,2);</div><div class="line"><a name="l01189"></a><span class="lineno"> 1189</span>&#160;                    <span class="keyword">const</span> TReal IL12 = -L(1,2);</div><div class="line"><a name="l01190"></a><span class="lineno"> 1190</span>&#160;                    <span class="keyword">const</span> TReal IL03 = L(0,1) * L(1,3) - L(0,3);</div><div class="line"><a name="l01191"></a><span class="lineno"> 1191</span>&#160;                    <span class="keyword">const</span> TReal IL13 = -L(1,3);</div><div class="line"><a name="l01192"></a><span class="lineno"> 1192</span>&#160;</div><div class="line"><a name="l01193"></a><span class="lineno"> 1193</span>&#160;                    <span class="comment">// Compute inverse of D.</span></div><div class="line"><a name="l01194"></a><span class="lineno"> 1194</span>&#160;                    <span class="comment">// Inverse[{{d_11,0,0,0},{0,d_22,0,0},{0,0,d_33,d_43},{0,0,d_43,d_44}}]</span></div><div class="line"><a name="l01195"></a><span class="lineno"> 1195</span>&#160;                    <span class="keyword">const</span> TReal ID00 = 1 / D(0,0);</div><div class="line"><a name="l01196"></a><span class="lineno"> 1196</span>&#160;                    <span class="keyword">const</span> TReal ID11 = 1 / D(1,1);</div><div class="line"><a name="l01197"></a><span class="lineno"> 1197</span>&#160;                    matrix&lt;TReal, 2, 2&gt; ID;</div><div class="line"><a name="l01198"></a><span class="lineno"> 1198</span>&#160;                    ID(0,0) = D(3,3);</div><div class="line"><a name="l01199"></a><span class="lineno"> 1199</span>&#160;                    ID(0,1) = -D(3,2);</div><div class="line"><a name="l01200"></a><span class="lineno"> 1200</span>&#160;                    ID(1,0) = -D(3,2);</div><div class="line"><a name="l01201"></a><span class="lineno"> 1201</span>&#160;                    ID(1,1) = D(2,2);</div><div class="line"><a name="l01202"></a><span class="lineno"> 1202</span>&#160;                    multiply(ID, 1 / DD);</div><div class="line"><a name="l01203"></a><span class="lineno"> 1203</span>&#160;</div><div class="line"><a name="l01204"></a><span class="lineno"> 1204</span>&#160;                    <span class="comment">// ### 11. if log10 |u22| &gt; -7.18</span></div><div class="line"><a name="l01205"></a><span class="lineno"> 1205</span>&#160;                    <span class="comment">// This implies that u22 &gt; 10^-7.18 ~= 6.607e-8</span></div><div class="line"><a name="l01206"></a><span class="lineno"> 1206</span>&#160;                    <span class="keyword">static</span> <span class="keyword">const</span> TReal kSubspaceThreshold = constants&lt;TReal&gt;::get_subspace_threshold();</div><div class="line"><a name="l01207"></a><span class="lineno"> 1207</span>&#160;                    <span class="keyword">const</span> TReal AU = math_utils&lt;TReal&gt;::fabs(u22);</div><div class="line"><a name="l01208"></a><span class="lineno"> 1208</span>&#160;                    <span class="keywordflow">if</span> (AU &gt; kSubspaceThreshold)</div><div class="line"><a name="l01209"></a><span class="lineno"> 1209</span>&#160;                    {</div><div class="line"><a name="l01210"></a><span class="lineno"> 1210</span>&#160;                        <span class="comment">// ### 12. nit = ceil(15/(16.86 + 2 log10 |u22|))</span></div><div class="line"><a name="l01211"></a><span class="lineno"> 1211</span>&#160;                        <span class="keyword">const</span> <span class="keywordtype">int</span> nit = <span class="keyword">static_cast&lt;</span><span class="keywordtype">int</span><span class="keyword">&gt;</span>(</div><div class="line"><a name="l01212"></a><span class="lineno"> 1212</span>&#160;                            math_utils&lt;TReal&gt;::ceil(15 / (static_cast&lt;TReal&gt;(16.8) + 2 * math_utils&lt;TReal&gt;::log10(AU)))</div><div class="line"><a name="l01213"></a><span class="lineno"> 1213</span>&#160;                            );</div><div class="line"><a name="l01214"></a><span class="lineno"> 1214</span>&#160;</div><div class="line"><a name="l01215"></a><span class="lineno"> 1215</span>&#160;                        <span class="comment">// ### 13. Compute Bs = LDL^T by block LDL^T factorization with Bunch-Parlett pivoting</span></div><div class="line"><a name="l01216"></a><span class="lineno"> 1216</span>&#160;                        <span class="comment">// Already done.</span></div><div class="line"><a name="l01217"></a><span class="lineno"> 1217</span>&#160;</div><div class="line"><a name="l01218"></a><span class="lineno"> 1218</span>&#160;                        <span class="comment">// ### 14. v = L^-T e4 / ||L^-T e4|| % Initial guess.</span></div><div class="line"><a name="l01219"></a><span class="lineno"> 1219</span>&#160;                        <span class="comment">//</span></div><div class="line"><a name="l01220"></a><span class="lineno"> 1220</span>&#160;                        <span class="comment">// | 1       IL01    IL02    IL03 |       | 0 |       | IL03 |</span></div><div class="line"><a name="l01221"></a><span class="lineno"> 1221</span>&#160;                        <span class="comment">// | 0       1       IL12    IL13 |   *   | 0 |   =   | IL13 |</span></div><div class="line"><a name="l01222"></a><span class="lineno"> 1222</span>&#160;                        <span class="comment">// | 0       0       1       0    |       | 0 |       | 0    |</span></div><div class="line"><a name="l01223"></a><span class="lineno"> 1223</span>&#160;                        <span class="comment">// | 0       0       0       1    |       | 1 |       | 1    |</span></div><div class="line"><a name="l01224"></a><span class="lineno"> 1224</span>&#160;                        v(0) = IL03;</div><div class="line"><a name="l01225"></a><span class="lineno"> 1225</span>&#160;                        v(1) = IL13;</div><div class="line"><a name="l01226"></a><span class="lineno"> 1226</span>&#160;                        v(2) = 0;</div><div class="line"><a name="l01227"></a><span class="lineno"> 1227</span>&#160;                        v(3) = 1;</div><div class="line"><a name="l01228"></a><span class="lineno"> 1228</span>&#160;                        <span class="comment">// Normalization will happen in the loop.</span></div><div class="line"><a name="l01229"></a><span class="lineno"> 1229</span>&#160;</div><div class="line"><a name="l01230"></a><span class="lineno"> 1230</span>&#160;                        <span class="comment">// ### 15. for i = 1 : nit</span></div><div class="line"><a name="l01231"></a><span class="lineno"> 1231</span>&#160;                        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; nit; ++i)</div><div class="line"><a name="l01232"></a><span class="lineno"> 1232</span>&#160;                        {</div><div class="line"><a name="l01233"></a><span class="lineno"> 1233</span>&#160;                            normalize(v);</div><div class="line"><a name="l01234"></a><span class="lineno"> 1234</span>&#160;</div><div class="line"><a name="l01235"></a><span class="lineno"> 1235</span>&#160;                            <span class="comment">// ### 16. Update v using one step of inverse iteration with LDL^T</span></div><div class="line"><a name="l01236"></a><span class="lineno"> 1236</span>&#160;                            <span class="comment">// OPTME: Maybe some of these operations could be combined to be optimized...?</span></div><div class="line"><a name="l01237"></a><span class="lineno"> 1237</span>&#160;</div><div class="line"><a name="l01238"></a><span class="lineno"> 1238</span>&#160;                            <span class="comment">// v = L^-1 * v = IL * v;</span></div><div class="line"><a name="l01239"></a><span class="lineno"> 1239</span>&#160;                            v = multiply_il_v(IL01, IL02, IL03, IL12, IL13, v);</div><div class="line"><a name="l01240"></a><span class="lineno"> 1240</span>&#160;</div><div class="line"><a name="l01241"></a><span class="lineno"> 1241</span>&#160;                            <span class="comment">// v = D^-1 = ID * v;</span></div><div class="line"><a name="l01242"></a><span class="lineno"> 1242</span>&#160;                            v = multiply_id_v(ID00, ID11, ID, v);</div><div class="line"><a name="l01243"></a><span class="lineno"> 1243</span>&#160;</div><div class="line"><a name="l01244"></a><span class="lineno"> 1244</span>&#160;                            <span class="comment">// v = L^-T * v = IL^T * v = v * IL;</span></div><div class="line"><a name="l01245"></a><span class="lineno"> 1245</span>&#160;                            v = multiply_v_il(v, IL01, IL02, IL03, IL12, IL13);</div><div class="line"><a name="l01246"></a><span class="lineno"> 1246</span>&#160;                        }</div><div class="line"><a name="l01247"></a><span class="lineno"> 1247</span>&#160;                        <span class="comment">// ### 17. end</span></div><div class="line"><a name="l01248"></a><span class="lineno"> 1248</span>&#160;                        <span class="comment">// The last normalization of v will be done at the end.</span></div><div class="line"><a name="l01249"></a><span class="lineno"> 1249</span>&#160;                    }</div><div class="line"><a name="l01250"></a><span class="lineno"> 1250</span>&#160;                    <span class="comment">// ### 18. else</span></div><div class="line"><a name="l01251"></a><span class="lineno"> 1251</span>&#160;                    <span class="keywordflow">else</span></div><div class="line"><a name="l01252"></a><span class="lineno"> 1252</span>&#160;                    {</div><div class="line"><a name="l01253"></a><span class="lineno"> 1253</span>&#160;                        <span class="comment">// ### 19. Compute Bs = LDL^T by block LDL^T factorization with Bunch-Parlett pivoting</span></div><div class="line"><a name="l01254"></a><span class="lineno"> 1254</span>&#160;                        <span class="comment">// Already done.</span></div><div class="line"><a name="l01255"></a><span class="lineno"> 1255</span>&#160;</div><div class="line"><a name="l01256"></a><span class="lineno"> 1256</span>&#160;                        <span class="comment">// ### 20. V = L^-T [e3 e4] % Initial guess.</span></div><div class="line"><a name="l01257"></a><span class="lineno"> 1257</span>&#160;                        <span class="comment">//</span></div><div class="line"><a name="l01258"></a><span class="lineno"> 1258</span>&#160;                        <span class="comment">// | 1       IL01    IL02    IL03 |       | 0    0 |       | IL02    IL03 |</span></div><div class="line"><a name="l01259"></a><span class="lineno"> 1259</span>&#160;                        <span class="comment">// | 0       1       IL12    IL13 |   *   | 0    0 |   =   | IL12    IL13 |</span></div><div class="line"><a name="l01260"></a><span class="lineno"> 1260</span>&#160;                        <span class="comment">// | 0       0       1       0    |       | 1    0 |       | 1       0    |</span></div><div class="line"><a name="l01261"></a><span class="lineno"> 1261</span>&#160;                        <span class="comment">// | 0       0       0       1    |       | 0    1 |       | 0       1    |</span></div><div class="line"><a name="l01262"></a><span class="lineno"> 1262</span>&#160;                        <span class="keyword">const</span> TReal v00 = IL02;</div><div class="line"><a name="l01263"></a><span class="lineno"> 1263</span>&#160;                        <span class="keyword">const</span> TReal v10 = IL03;</div><div class="line"><a name="l01264"></a><span class="lineno"> 1264</span>&#160;                        <span class="keyword">const</span> TReal v01 = IL12;</div><div class="line"><a name="l01265"></a><span class="lineno"> 1265</span>&#160;                        <span class="keyword">const</span> TReal v11 = IL13;</div><div class="line"><a name="l01266"></a><span class="lineno"> 1266</span>&#160;</div><div class="line"><a name="l01267"></a><span class="lineno"> 1267</span>&#160;                        <span class="comment">// ### 21. for i = 1:2</span></div><div class="line"><a name="l01268"></a><span class="lineno"> 1268</span>&#160;                        <span class="comment">// ### 22. Orthonormalize V via QR factorization.</span></div><div class="line"><a name="l01269"></a><span class="lineno"> 1269</span>&#160;                        <span class="comment">// ### 23. Update V using one step of inverse subspace iteration with LDL^T.</span></div><div class="line"><a name="l01270"></a><span class="lineno"> 1270</span>&#160;                        vector&lt;TReal, 4&gt; v0, v1;</div><div class="line"><a name="l01271"></a><span class="lineno"> 1271</span>&#160;                        orthonormalize_v_with_qr(v0, v1, v00, v10, v01, v11);</div><div class="line"><a name="l01272"></a><span class="lineno"> 1272</span>&#160;</div><div class="line"><a name="l01273"></a><span class="lineno"> 1273</span>&#160;                        <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; 2; ++i)</div><div class="line"><a name="l01274"></a><span class="lineno"> 1274</span>&#160;                        {</div><div class="line"><a name="l01275"></a><span class="lineno"> 1275</span>&#160;                            v0 = multiply_il_v(IL01, IL02, IL03, IL12, IL13, v0);</div><div class="line"><a name="l01276"></a><span class="lineno"> 1276</span>&#160;                            v1 = multiply_il_v(IL01, IL02, IL03, IL12, IL13, v1);</div><div class="line"><a name="l01277"></a><span class="lineno"> 1277</span>&#160;</div><div class="line"><a name="l01278"></a><span class="lineno"> 1278</span>&#160;                            v0 = multiply_id_v(ID00, ID11, ID, v0);</div><div class="line"><a name="l01279"></a><span class="lineno"> 1279</span>&#160;                            v1 = multiply_id_v(ID00, ID11, ID, v1);</div><div class="line"><a name="l01280"></a><span class="lineno"> 1280</span>&#160;</div><div class="line"><a name="l01281"></a><span class="lineno"> 1281</span>&#160;                            v0 = multiply_v_il(v0, IL01, IL02, IL03, IL12, IL13);</div><div class="line"><a name="l01282"></a><span class="lineno"> 1282</span>&#160;                            v1 = multiply_v_il(v1, IL01, IL02, IL03, IL12, IL13);</div><div class="line"><a name="l01283"></a><span class="lineno"> 1283</span>&#160;                        }</div><div class="line"><a name="l01284"></a><span class="lineno"> 1284</span>&#160;                        <span class="comment">// ### 24. end</span></div><div class="line"><a name="l01285"></a><span class="lineno"> 1285</span>&#160;</div><div class="line"><a name="l01286"></a><span class="lineno"> 1286</span>&#160;                        <span class="comment">// ### 25. Orthonormalize V via QR factorization.</span></div><div class="line"><a name="l01287"></a><span class="lineno"> 1287</span>&#160;                        orthonormalize_v_with_qr(v0, v1);</div><div class="line"><a name="l01288"></a><span class="lineno"> 1288</span>&#160;</div><div class="line"><a name="l01289"></a><span class="lineno"> 1289</span>&#160;                        <span class="comment">// ### 26. Bp = V^T Bs V in R2x2</span></div><div class="line"><a name="l01290"></a><span class="lineno"> 1290</span>&#160;                        <span class="comment">// ### 27. Find w, eigenvector of smallest eigenvalue of Bp, by analytic formula.</span></div><div class="line"><a name="l01291"></a><span class="lineno"> 1291</span>&#160;                        <span class="comment">// ### 28. v = V w</span></div><div class="line"><a name="l01292"></a><span class="lineno"> 1292</span>&#160;                        <span class="comment">// L has the same form as IL, so we can use the same function to multiply them.</span></div><div class="line"><a name="l01293"></a><span class="lineno"> 1293</span>&#160;                        <span class="keyword">const</span> vector&lt;TReal, 4&gt; v0_temp = multiply_v_il(v0, L(0,1), L(0,2), L(0,3), L(1,2), L(1,3));</div><div class="line"><a name="l01294"></a><span class="lineno"> 1294</span>&#160;                        <span class="keyword">const</span> vector&lt;TReal, 4&gt; v1_temp = multiply_v_il(v1, L(0,1), L(0,2), L(0,3), L(1,2), L(1,3));</div><div class="line"><a name="l01295"></a><span class="lineno"> 1295</span>&#160;                        <span class="keyword">const</span> vector&lt;TReal, 4&gt; H0 = multiply_minus_v_d(v0_temp, D);</div><div class="line"><a name="l01296"></a><span class="lineno"> 1296</span>&#160;                        <span class="keyword">const</span> vector&lt;TReal, 4&gt; H1 = multiply_minus_v_d(v1_temp, D);</div><div class="line"><a name="l01297"></a><span class="lineno"> 1297</span>&#160;                        <span class="keyword">const</span> TReal H00 = dot(H0, v0_temp);</div><div class="line"><a name="l01298"></a><span class="lineno"> 1298</span>&#160;                        <span class="keyword">const</span> TReal H10 = dot(H0, v1_temp);</div><div class="line"><a name="l01299"></a><span class="lineno"> 1299</span>&#160;                        <span class="keyword">const</span> TReal H01 = dot(H1, v0_temp);</div><div class="line"><a name="l01300"></a><span class="lineno"> 1300</span>&#160;                        <span class="keyword">const</span> TReal H11 = dot(H1, v1_temp);</div><div class="line"><a name="l01301"></a><span class="lineno"> 1301</span>&#160;                        <span class="keywordflow">if</span> (math_utils&lt;TReal&gt;::fabs(H10) &lt; static_cast&lt;TReal&gt;(1.0e-15))</div><div class="line"><a name="l01302"></a><span class="lineno"> 1302</span>&#160;                        {</div><div class="line"><a name="l01303"></a><span class="lineno"> 1303</span>&#160;                            <span class="keywordflow">if</span> (H00 &gt; H10)</div><div class="line"><a name="l01304"></a><span class="lineno"> 1304</span>&#160;                                v = v0;</div><div class="line"><a name="l01305"></a><span class="lineno"> 1305</span>&#160;                            <span class="keywordflow">else</span></div><div class="line"><a name="l01306"></a><span class="lineno"> 1306</span>&#160;                                v = v1;</div><div class="line"><a name="l01307"></a><span class="lineno"> 1307</span>&#160;                        }</div><div class="line"><a name="l01308"></a><span class="lineno"> 1308</span>&#160;                        <span class="keywordflow">else</span></div><div class="line"><a name="l01309"></a><span class="lineno"> 1309</span>&#160;                        {</div><div class="line"><a name="l01310"></a><span class="lineno"> 1310</span>&#160;                            <span class="keyword">const</span> TReal r = (H00 - H11) / (2 * H10);</div><div class="line"><a name="l01311"></a><span class="lineno"> 1311</span>&#160;                            <span class="keyword">const</span> <span class="keywordtype">int</span> s = (H10 &lt; 0 ? -1 : 1);</div><div class="line"><a name="l01312"></a><span class="lineno"> 1312</span>&#160;                            <span class="keyword">const</span> TReal f = r + s * math_utils&lt;TReal&gt;::sqrt(1 + r * r);</div><div class="line"><a name="l01313"></a><span class="lineno"> 1313</span>&#160;                            v(0) = v0(0) * f + v1(0);</div><div class="line"><a name="l01314"></a><span class="lineno"> 1314</span>&#160;                            v(1) = v0(1) * f + v1(1);</div><div class="line"><a name="l01315"></a><span class="lineno"> 1315</span>&#160;                            v(2) = v0(2) * f + v1(2);</div><div class="line"><a name="l01316"></a><span class="lineno"> 1316</span>&#160;                            v(3) = v0(3) * f + v1(3);</div><div class="line"><a name="l01317"></a><span class="lineno"> 1317</span>&#160;                        }</div><div class="line"><a name="l01318"></a><span class="lineno"> 1318</span>&#160;                    }</div><div class="line"><a name="l01319"></a><span class="lineno"> 1319</span>&#160;                    <span class="comment">// ### 29. end</span></div><div class="line"><a name="l01320"></a><span class="lineno"> 1320</span>&#160;                }</div><div class="line"><a name="l01321"></a><span class="lineno"> 1321</span>&#160;</div><div class="line"><a name="l01322"></a><span class="lineno"> 1322</span>&#160;                <span class="comment">// ### 30. Form the matrix Q from v as in Theorem 2.5.</span></div><div class="line"><a name="l01323"></a><span class="lineno"> 1323</span>&#160;                <span class="comment">// ### 31. Compute H = Q^T A.</span></div><div class="line"><a name="l01324"></a><span class="lineno"> 1324</span>&#160;                <span class="comment">// Both are done at the end of the function.</span></div><div class="line"><a name="l01325"></a><span class="lineno"> 1325</span>&#160;            }</div><div class="line"><a name="l01326"></a><span class="lineno"> 1326</span>&#160;</div><div class="line"><a name="l01327"></a><span class="lineno"> 1327</span>&#160;            <span class="comment">// Compute rotation from dominant eigen vector v.</span></div><div class="line"><a name="l01328"></a><span class="lineno"> 1328</span>&#160;            normalize(v);</div><div class="line"><a name="l01329"></a><span class="lineno"> 1329</span>&#160;            vector&lt;TReal, 4&gt; vtemp = v;</div><div class="line"><a name="l01330"></a><span class="lineno"> 1330</span>&#160;            v(p(0)) = vtemp(0);</div><div class="line"><a name="l01331"></a><span class="lineno"> 1331</span>&#160;            v(p(1)) = vtemp(1);</div><div class="line"><a name="l01332"></a><span class="lineno"> 1332</span>&#160;            v(p(2)) = vtemp(2);</div><div class="line"><a name="l01333"></a><span class="lineno"> 1333</span>&#160;            v(p(3)) = vtemp(3);</div><div class="line"><a name="l01334"></a><span class="lineno"> 1334</span>&#160;</div><div class="line"><a name="l01335"></a><span class="lineno"> 1335</span>&#160;            <span class="keyword">const</span> TReal v12 = 2 * v(0) * v(1);</div><div class="line"><a name="l01336"></a><span class="lineno"> 1336</span>&#160;            <span class="keyword">const</span> TReal v13 = 2 * v(0) * v(2);</div><div class="line"><a name="l01337"></a><span class="lineno"> 1337</span>&#160;            <span class="keyword">const</span> TReal v14 = 2 * v(0) * v(3);</div><div class="line"><a name="l01338"></a><span class="lineno"> 1338</span>&#160;            <span class="keyword">const</span> TReal v22 = 2 * v(1) * v(1);</div><div class="line"><a name="l01339"></a><span class="lineno"> 1339</span>&#160;            <span class="keyword">const</span> TReal v23 = 2 * v(1) * v(2);</div><div class="line"><a name="l01340"></a><span class="lineno"> 1340</span>&#160;            <span class="keyword">const</span> TReal v24 = 2 * v(1) * v(3);</div><div class="line"><a name="l01341"></a><span class="lineno"> 1341</span>&#160;            <span class="keyword">const</span> TReal v33 = 2 * v(2) * v(2);</div><div class="line"><a name="l01342"></a><span class="lineno"> 1342</span>&#160;            <span class="keyword">const</span> TReal v34 = 2 * v(2) * v(3);</div><div class="line"><a name="l01343"></a><span class="lineno"> 1343</span>&#160;            <span class="keyword">const</span> TReal v44 = 2 * v(3) * v(3);</div><div class="line"><a name="l01344"></a><span class="lineno"> 1344</span>&#160;</div><div class="line"><a name="l01345"></a><span class="lineno"> 1345</span>&#160;            paramQ(0,0) = 1 - (v33 + v44);</div><div class="line"><a name="l01346"></a><span class="lineno"> 1346</span>&#160;            paramQ(0,1) = v23 - v14;</div><div class="line"><a name="l01347"></a><span class="lineno"> 1347</span>&#160;            paramQ(0,2) = v24 + v13;</div><div class="line"><a name="l01348"></a><span class="lineno"> 1348</span>&#160;            paramQ(1,0) = v23 + v14;</div><div class="line"><a name="l01349"></a><span class="lineno"> 1349</span>&#160;            paramQ(1,1) = 1 - (v22 + v44);</div><div class="line"><a name="l01350"></a><span class="lineno"> 1350</span>&#160;            paramQ(1,2) = v34 - v12;</div><div class="line"><a name="l01351"></a><span class="lineno"> 1351</span>&#160;            paramQ(2,0) = v24 - v13;</div><div class="line"><a name="l01352"></a><span class="lineno"> 1352</span>&#160;            paramQ(2,1) = v34 + v12;</div><div class="line"><a name="l01353"></a><span class="lineno"> 1353</span>&#160;            paramQ(2,2) = 1 - (v22 + v33);</div><div class="line"><a name="l01354"></a><span class="lineno"> 1354</span>&#160;</div><div class="line"><a name="l01355"></a><span class="lineno"> 1355</span>&#160;            <span class="comment">// The Matlab implementation returns the opposite of the matrix if det A &lt; 0.</span></div><div class="line"><a name="l01356"></a><span class="lineno"> 1356</span>&#160;            <span class="comment">// We don&#39;t do that because we want a right-handed rotation.</span></div><div class="line"><a name="l01357"></a><span class="lineno"> 1357</span>&#160;</div><div class="line"><a name="l01358"></a><span class="lineno"> 1358</span>&#160;            <span class="comment">// Compute scale.</span></div><div class="line"><a name="l01359"></a><span class="lineno"> 1359</span>&#160;            transpose_multiply(paramH, paramQ, paramA);</div><div class="line"><a name="l01360"></a><span class="lineno"> 1360</span>&#160;</div><div class="line"><a name="l01361"></a><span class="lineno"> 1361</span>&#160;            <span class="comment">// The Matlab implementation suggests averaging the top and lower part of the</span></div><div class="line"><a name="l01362"></a><span class="lineno"> 1362</span>&#160;            <span class="comment">// matrix to ensure symmetry, but we don&#39;t do it.</span></div><div class="line"><a name="l01363"></a><span class="lineno"> 1363</span>&#160;        }</div><div class="line"><a name="l01364"></a><span class="lineno"> 1364</span>&#160;</div><div class="line"><a name="l01365"></a><span class="lineno"> 1365</span>&#160;</div><div class="line"><a name="l01366"></a><span class="lineno"> 1366</span>&#160;    }; <span class="comment">// End of namespace detail.</span></div><div class="line"><a name="l01367"></a><span class="lineno"> 1367</span>&#160;</div><div class="line"><a name="l01368"></a><span class="lineno"> 1368</span>&#160;</div><div class="line"><a name="l01369"></a><span class="lineno"> 1369</span>&#160;}; <span class="comment">// End of namespace polar.</span></div><div class="line"><a name="l01370"></a><span class="lineno"> 1370</span>&#160;</div><div class="line"><a name="l01371"></a><span class="lineno"> 1371</span>&#160;</div><div class="line"><a name="l01372"></a><span class="lineno"> 1372</span>&#160;</div><div class="line"><a name="l01373"></a><span class="lineno"> 1373</span>&#160;</div><div class="line"><a name="l01374"></a><span class="lineno"> 1374</span>&#160;<span class="preprocessor">#endif // __POLAR_DECOMPOSITION_3X3_IMPL_H__</span></div><div class="ttc" id="structpolar_1_1detail_1_1constants_html"><div class="ttname"><a href="structpolar_1_1detail_1_1constants.html">polar::detail::constants</a></div><div class="ttdef"><b>Definition:</b> polar_decomposition_3x3_impl.h:787</div></div>
<div class="ttc" id="namespacepolar_html"><div class="ttname"><a href="namespacepolar.html">polar</a></div><div class="ttdef"><b>Definition:</b> polar_decomposition_3x3.h:32</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.14
</small></address>
</body>
</html>
